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PREFACE 


This  document  specifies  the  detailed  design  of  the  missile  software  parts 
identified  during  the  Common  Ada  Missile  Packages  (CAMP)  program. 

This  document  contains  data  generated  under  the  work  task  described  by  sections 
5. 3. 1.2  and  5. 3. 2. 3  of  DOD-STD-2167,  Defense  System  Software  Development.  It 
has  been  prepared  in  accordance  with  DI-MCCR-80031,  Software  Detailed  Design 
Document. 

The  structure  of  DI-MCCR-80031  requires  tailoring  to  meet  the  needs  of  reusable 
software  components.  The  DID  is  intended  for  use  in  describing  the  design  of  a 
single  application  Computer  Software  Configuration  Item  (CSCI).  The  DID  has, 
therefore,  been  tailored  for  describing  the  design  of  parts  used  individually 
or  on  a  group  basis.  In  addition,  elements  of  DI-MCCR-80031  were  already 
incorporated  into  the  paragraph  structure  of  the  Top-Level  Design  Document 
(TLDD)  because  the  DID  for  that  document  does  not  provide  enough  detail  to  be 
useful  as  a  top-level  design  description  for  reusable  parts.  Appendix  I 
summarizes  the  modifications  made  to  this  DID  to  tailor  it  for  the  description 
of  the  detailed  design  of  reusable  missile  software  parts. 
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SOFTWARE  DETAILED  DESIGN  DOCUMENT 
FOR  THE 

MISSILE  SOFTWARE  PARTS 
OF  THE 

COMMON  ADA  MISSILE  PACKAGES  (CAMP) 
PROJECT 


1  SCOPE 

1.1  IDENTIFICATION 

This  Software  Detailed  Design  Document  describes  the  detailed  design  for  the 
CSCI  identified  as  the  Common  Ada  Missile  Packages  (CAMP)  Missile  Software 
Parts. 

The  CAMP-1  program  was  a  twelve-month  feasibility  study  which  the  McDonnell 
Douglas  Astronautics  Company  performed  under  contract  to  the  U.S.  Air  Force 
Armament  Laboratory  (AFATL).  This  project  had  two  primary  objectives: 

(1)  To  determine  the  feasibility  of  developing  reusable  missile  software 
components  written  in  Ada;  and, 

(2)  To  determine  the  feasibility  of  developing  an  automated  or 
semi-automated  missile  software  generation  system. 

The  CAMP-2  program  was  a  twenty-four-month  study  which  the  McDonnell  Douglas 
Astronautics  Company  performed  under  contract  to  the  U.S.  Air  Force  Armament 
Laboratory  (AFATL).  The  primary  objectives  of  this  project  were: 

(1)  Prepare  the  detailed  design  for  those  parts  receiving  top-level  design 
under  the  CAMP-1  program. 

(2)  Show  that  reusable  software  parts  can  result  in  significant 
productivity  and  quality  improvements. 

(3)  Implement  an  11th  missile  application  using  CAMP  parts. 

The  detailed  design  of  this  CSCI  meets  the  requirements  which  resulted  from  the 
domain  analysis  performed  during  CAMP-1.  Although  software  parts  contained  in 
this  CSCI  are  used  by  the  software  generator,  there  are  no  direct  software  or 
hardware  interfaces  between  the  two  CSCI's. 


1.2  PURPOSE 

The  Missile  Software  Parts  constitute  a  set  of  common  software  identifed  under 
the  domain  analysis  of  the  CAMP  study.  These  software  parts  are  grouped  into 
Top-Level  Computer  Software  Components  (TLCSC's)  and  are  divided  into  the 
categories  listed  below. 

a.  Data  Types  -  TLCSC's  which  provide  data  types  used  in  other  TLCSC's  or 
in  a  user  application: 
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g- 


o  Basic  Data  Types  TLCSC 
o  Kalman  Filter  Data  Types  TLCSC 
o  Autopilot  Data  Types  TLCSC 


b. 


Navigation  -  TLCSC' s  which  provide  the  basic  functionality  of  a 
navigation  sybsystem. 


o 

o 

o 

0 


Common  Navigation  Parts  TLCSC 
North  Pointing  Navigation  Parts  TLCSC 
Wander  Angle  Navigation  Parts  TLCSC 
Direction  Cosine  Matrix  Operations  TLCSC 


c.  Kalman  Filter  -  TLCSC's  which  provide  common  Kalman  Filter  functions. 


o  Common  Kalman  Filter  TLCSC 
o  Compact-H  Kalman  Filter  TLCSC 
o  Complicated-H  Kalman  Filter  TLCSC 


d.  Guidance  and  Control  -  TLCSC's  which  provide  the  basic  functionality 


of  a  guidance  and  control  subsystem. 


Waypoint  Steering  TLCSC 
Autopilot  TLCSC 


e. 


Nonguidance  Control  -  TLCSC's  which  provide  the  basic  functionality  of 
a  control  subsystem  for  operations  outside  of  the  guidance  area. 


o 

o 


Air  Data  TLCSC 
Fuel  Control  TLCSC 


f.  Mathematical  -  TLCSC's  which  provide  a  variety  of  useful  mathematical 


functions  such  as  coordinate  and  matrix  algebra,  trigonometric,  and 
signal  processing  functions. 


o 

o 

o 

o 

o 

o 

o 

o 

o 

o 


Coordinate  Algebra  TLCSC 
Matrix  Algebra  TLCSC 
Geometric  TLCSC 
Trigonometry  TLCSC 
Unit  Conversion  TLCSC 
External  Form  Conversion  TLCSC 
Signal  Processing  TLCSC 
General  Purpose  Math  TLCSC 
Polynomial  Math  TLCSC 
Quaternion  Math  TLCSC 


Abstract  Mechanisms  -  TLCSC's  which  provide  abstract  data  structures 
and  processes. 


o  Data  Structures  TLCSC 


h.  General  Utilities  -  TLCSC's  which  provide  other  functions  needed  for 
missile  or  other  weapons  system  operation. 


o  General  Utilities  TLCSC 
o  Communication  Parts  TLCSC 
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i.  Equipment  Interfaces  -  TLCSC's  which  provide  functions  needed  to 
interface  with  the  hardware: 


o  Clock  Handler  TLCSC 

The  following  lists  the  TLCSC's  identified  in  the  CAMP  Top-Level  Design 
Document  for  which  no  detailed  design  exists.  Detailed  design  is  not  required 
for  these  parts  either  because  they  deal  exclusively  with  data  constants  or 
because  they  are  schematic  parts. 

a.  Data  constants  -  TLCSC's  which  provide  data  constants  used  in  a 
typical  application: 

o  WGS-72  Engineering  TLCSC 
o  WGS-72  Metric  TLCSC 
o  WGS-72  Unitless  TLCSC 
o  Universal  Constants  TLCSC 
o  Conversion  Factors  TLCSC 


b.  Equipment  Interfaces  -  TLCSC's  which  provide  standard  interfaces  to 
specific  hardware  components  or  to  general  classes  of  hardware. 

o  Radar  Altimeter  Interface  TLCSC's 

o  Data  Bus  Handler  TLCSC 

c.  Abstract  Mechanisms  -  TLCSC's  which  provide  abstract  data  structures 
and  processes . 

o  Abstract  Processes  TLCSC  (schematic  part) 

d.  Process  Management  -  TLCSC's  which  provide  control  of  concurrent 
processes  ana  communication. 

o  Asynchronous  Control  TLCSC  (schematic  part) 


1.3  INTRODUCTION 

This  Software  Detailed  Design  Document  (DDD)  provides  the  detailed  design 
requirements  of  common  missile  software  parts  as  stated  in  the  Software 
Requirements  Specification  developed  under  Contract  F08635-84-C-0280  of  the 
CAMP  Program. 

The  following  figure  shows  the  organization  of  TLCSC's  requiring  detailed 
design  and  the  structure  for  the  major  section  of  this  document,  Section  3.3 
(Detailed  Design). 
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DATA  TIPIS . 3.3.1 

Bade  Data  Typaa  TLCSC . 3. 3. 1.1 

Kalaan  riltar  Data  Typaa  TLCSC . 3. 3. 1.2 

Autopilot  Data  Typaa  TLCSC . 3. 3. 1.3 

MAVIQATION . 3.3.2 

Coaaon  navigation  Pacts  TLCSC  .  .  .  3. 3. 2.1 

Rorth  Pointing  navigation  Parts  TLCSC  .  3. 3. 2. 2 

Wandar  Angla  navigation  Parts  TLCSC . 3. 3. 2. 3 

Diraction  Cosina  Matrix  Operation*  TLCSC  .  3. 3. 2. 4 


KALMAN  FILTER  .  3.3.3 

Coaaon  Kalaan  riltar  Parts  TLCSC  . . 3. 3. 3.1 

Coapact-3  Kalaan  riltar  Parts  TLCSC . .  3. 3. 3. 2 

Coaplicatad-H  Kalaan  riltar  Parts  TLCSC  .  3. 3. 3. 3 


GUIDANCE  AMD  COPTAOL . 3.3.4 

Waypoint  Staaring  TLCSC  .  3. 3. 4.1 

Autopilot  TLCSC  .  3. 3. 4. 2 


nonoui DANCE  AND  COnTXOL  .  3.3.5 

Air  Data  TLCSC . 3. 3. 5.1 

rual  Control  TLCSC  .  3. 3. 5. 2 

MATHEMATICAL . 3.3.6 

Coordinata  Vactor/Matrix  Algabra  TLCSC  .  3. 3. 6.1 

General  Vactor/Matrix  Algebra  TLCSC . . . 3. 3. 6. 2 

Trigoncaatric  TLCSC  .  3. 3. 6. 3 

Aeoaetric  Algabra  TLCSC . 3. 3. 6. 4 

Data  Conversion . 3. 3. 6. 5 

Unit  conversion  TLCSC  .  3. 3. 6. 5.1 

External  Cora  conversion  TLCSC . 3. 3. 6. 5. 2 

Signal  Processing  TLCSC  .  .  .  J.o.S.6 

General  Purpose  Math  TLCSC  .  3. 3. 6. 7 

Polynomial  TLCSC  .  3.316.1 

Quaternion  Operations  TLCSC  .  3. 3. 6. 9 

ABSTHACT  MECHANISMS . 3.3.7 

Abstract  Data  Structures  TLCSC  .  3. 3. 7.1 

aiMEXAL  UTILITIES . 3.3.1 

General  Utilities  TLCSC  .  3.3.S.1 

Coaaunication  TLCSC  .  3.3.S.2 

EQUIPMENT  inTEEPACES  .  3.3.9 

Clock  Handler  TLCSC  .  3. 3. 9.1 


ORGANIZATION  OF  CAMP  TLCSC' S 
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The  following  figure  illustrates  the  top-level  architecture  and  dependencies  of 
the  TLCSC' s.  The  top  section  of  this  figure  shows  the  mathematical  and  data 
type  packages.  These  TLCSC' s  provide  the  data  types  and  mathematical 
operations  to  a  user  program  or  to  other  TLCSC' s  within  the  design*  as  shown  by 
the  arrows.  At  the  center  of  the  figure  are  the  primary  missile  TLCSC's: 
Navigation,  Kalman  Filter,  Guidance  and  Control,  and  Non-guidance  Control. 

These  TLCSC's  require  data  types  and  mathematical  operators  from  other  TLCSC's 
and  provide  operations  to  the  user  program.  On  the  right  of  the  figure  are 
secondary  missile  TLCSC's.  These  also  provide  operations  to  a  user  program,  as 
illustrated  by  the  arrows. 
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2  APPLICABLE  DOCUMENTS 
2.1  GOVERNMENT  DOCUMENTS 

The  following  documents  of  the  exact  issue  shown  form  a  part  of  the  specifica¬ 
tion  to  the  extent  described  herein.  In  the  event  of  conflict  between  the 
documents  referenced  herein  and  the  contents  of  this  document,  the  contents  of 
this  document  shall  be  considered  a  superseding  requirement. 

STANDARDS 

MILITARY 

MIL-STD-490 

MIL-STD-1815A 

DOD-STD-2167 

OTHER  PUBLICATIONS 

CAMP  Final  Technical  Report,  Volume  1,  2,  &  3  (4  September  1985) 

Ada  Missile  Parts  Engineering  Expert  System  Software  Requirements 
Specification  (4  September  1985) 

Ada  Missile  Parts  Engineering  Expert  System  Software  Top-Level  Design 
Document  (4  September  1985) 

Ada  Missile  Parts  Engineering  Expert  System  Software  Detailed  Design 
Document  (Draft  30  August  1986) 

CAMP  Missile  Software  Parts  Requirements  Specification 
(4  September  1985) 

CAMP  Missile  Software  Parts  Software  Top-Level  Design  Document 
(4  September  1985) 

World  Geodetic  Survey,  1972  (WGS72).  Defense  Mapping  Agency,  DMA  72-1 


Specification  Practices  (4  June  1985) 

Reference  Manual  for  the  Ada  Programming 
Language  (17  February  1983) 

Defense  System  Software  Development 
(4  June  1985) 


2.2  NON-GOVERNMENT  DOCUMENTS 

The  following  documents  of  the  exact  issue  shown  form  a  part  of  the  specifica¬ 
tion  to  the  extend  specified  herein.  In  the  event  of  conflict  between  the 
documents  referenced  herein  and  the  contents  of  this  specification,  the 
contents  of  this  specification  shall  be  considered  a  superseding  requirement. 

STANDARDS 

Ada  Design  Language  Reference  Manual  ((c)  1983,  McDonnell  Douglas 
Astronautics  Company) 

OTHER  PUBLICATIONS: 


M  ,  y  *  vyvjj*- 
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VAX  Ada  Programmer's  Run-Time  Reference  Manual  (February,  1985,  Digital 
Equipment  Corporation) 

VAX  Ada  Language  Reference  Manual  (February,  1985,  Digital  Equipment 
Corporation) 
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3  REQUIREMENTS 
3.1  INTERFACE  DESIGN 

There  are  no  interfaces  between  this  CSCI  and  other  CSCI's  or  Cl's.  Interfaces 
between  TLCSC's  are  described  under  Section  3.3  (Detailed  Design). 


3.2  GLOBAL  DATA  AND  DATA  TYPES 

Global  data  and  data  types  are  provided  by  TLCSC's  in  the  categories  of  data 
constants  and  data  types.  The  specific  global  objects  and  types  declared  in 
these  TLCSC's  are  discussed  in  sections  3.6.1  of  the  Software  Top-Level  Design 
Document  (Data  Constants)  and  3.3.1  of  this  document  (Data  Types). 


A 
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3.3  DETAILED  DESIGN 
3.3.1  DATA  TYPES 
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3.3. 1.1  BASIC  DATA  TYPES  (BODY)  TLCSC  P621  (CATALOG  #P1093-0) 

This  part,  which  is  designed  as  an  Ada  package,  defines  simple  data  types  and 
special  operators  for  these  data  types. 

The  decomposition  for  this  part  is  the  same  as  that  shown  in  the  Top-Level 
Design  Document. 


3. 3. 1.1.1  REQUIREMENTS  ALLOCATION 
This  part  meets  CAMP  requirement  R021. 


|  3. 3. 1.1. 2  LOCAL  ENTITIES  DESIGN 

I  None. 


INPUT/OUTPUT 


LOCAL  DATA 


PROCESS  CONTROL 
Not  applicable. 


\  3. 3. 1.1. 6  PROCESSING 

J  The  following  describes  the  processing  performed  by  this  part: 

with  Conversion_Factors; 
package  body  Basic_Data_Types  is 

!  package  CF  renames  Conversion_Factors; 

>  use  Trig; 

i 

\  — - 

!  - Define  Operators  for  Basic  Data  Types- 


© 


3. 3. 1.1. 3 
None. 

3. 3. 1.1. 4 
None. 

3. 3. 1.1. 5 
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- Acceleration  *  Interval  ==>  Velocity 

function  (Left  :  Feet  per_Second_Squared; 

Right  :  Seconds) 
return  Feet  Per_Second  is 
Temp  :  Feet_Per_Seconcf; 
begin 

Temp  :=  Feet_Per_Second  (Seconds  (Left)  *  Right); 
return  Temp; 
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end 

function  (Left  :  Meters_per_Second_Squared; 

Right  :  Seconds) 
return  Meters_Per_Second  is 
Temp  :  Meters_Per_Second; 
begin 

Temp  :=  Meters_Per_Second  (Seconds  (Left)  *  Right); 
return  Temp; 
end 

- Acceleration  *  Sin  Cos  Ratio  ==>  Acceleration 
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function  "*'*  (Left  :  Feet_per_Second_Squared; 

Right  :  Trig.Sin_Cos_Ratio)  return  Feet_per_Second_Squared  is 

begin 

return  Left  *  Feet_per_Second_Squared  (Right); 
end 


function  (Left  :  Meters_per_Second_Squared; 

Right  :  Trig.Sin_Cos_Ratio) 
return  Meters_per_Second_Squared  is 

begin 

return  Left  *  Meters_per_Second_Squared  (Right); 
end 

'  function  (Left  :  Gees; 

Right  :  Trig.SinCosJtatio)  return  Gees  is 

begin 

return  Left  *  Gees  (Right); 

end 


? 

3 

s 


- Acceleration  *  Tan  Ratio  ==>  Acceleration 

function  "*M  (Left  :  Feet_per_Second_Squared; 

Right  :  Trig.Tan_Ratio)  return  Feet_per_Second_Squared  is 

begin 

return  Left  *  Feet_per_Second_Squared  (Right); 

end 


function  (Left  :  Meters_per_Second_Squared; 

Right  :  Trig.Tan_Ratio)  return  Meters_per_Second_Squared  is 

begin 

return  Left  *  Meters_per_Second_Squared  (Right); 

end 


function  '**"  (Left  :  Gees;  Right  ;  Trig.Tan_Ratio)  return  Gees  is 
begin 

return  Left  *  Gees  (Right); 
end 


- Angular_Veloci ty  *  Angular_Velocity  ==>  Angular_Velocity  Squared 


function  (Left  :  Radians_per_Second; 

Right  s  Radians_per_Second) 
return  Radians_Squared_per_Second_Squared  is 
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Temp  :  Radians_per_Second; 
begin 

Temp  :=  Left  *  Right; 

return  Radians_Squared_per_Second_Squared  (Temp); 
end  "*"; 

function  (Left  :  Degrees_per_Second; 

Right  :  Degrees_per_Second) 
return  Degrees  Squared_per_Second_Squared  is 
Temp  :  Degrees_per_Secon<l; 
begin 

Temp  :=  Left  *  Right; 

return  Degrees_Squared_per_Second_Squared  (Temp); 
end 

function  "*n  (Left  :  Semicircles_per_Second; 

Right  s  Semicircles_per_Second) 
return  Semicircles  Squared_per_Second_Squared  is 
Temp  :  Semicircles_per_Secon3; 
begin 

Temp  :=  Left  *  Right; 

return  Semicircles_Squared_per_Second_Squared  (Temp); 
end 

- Angular  Velocity  /  Sin  Cos  Ratio  =«->  Angular  Velocity 

function  "/"  (Left  :  Radians PerSecond; 

Right  :  Trig.Sin_Cos_Ratio) 
return  RadiansJPerSecond  is 

begin 

return  Left  /  Radians_Per_Second  (Right); 
end  "/"; 

- Angular  Velocity  *  Tan_Ratio  =■*>  Angular  Velocity 

function  (Left  :  Radians_Per_Second; 

Right  :  Trig.Tan_Ratio) 
return  Radians_Per_Second  is 

begin 

return  Left  *  Radians_Per_Second  (Right); 
end 

- Angular_Velocity  *  TimeJEnterval  =■=>  Angle 

function  (Left  :  Degrees_Per_Second; 

Right  :  SecondsJ  return  Trig. Degrees  is 

begin 

return  Trig. Degrees  (Right  *  Seconds  (Left)); 
end 

function  (Left  :  Radians_Per_Second; 

Right  :  SecondsJ  return  Trig. Radians  is 

begin 

return  Trig. Radians  (Right  *  Seconds  (Left)); 
end 

- Angular  Velocity  *  Time  Interval  ==>  Earth  Position 
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function  (Left  :  Radians_Per_Second; 

Right  :  Seconds) 

return  Earth_Position_Radians  is 

begin 

return  Earth_Position_Radians  (Seconds  (Left)  *  Right); 
end 

—  — Angular_Velocity  *  Velocity  ==>  Acceleration 

function  (Left  :  Radians_per_Second; 

Right  :  Meters_Per_Second) 
return  Meters  Per_Second_Squared  is 
Temp  :  Meters_Per_Secon<I; 
begin 

Temp  :=  Meters_Per_Second  (Left)  *  Right; 
return  Meters_Per_Second_Squared  (Temp); 
end 

function  (Left  ;  Radians_per_Second; 

Right  :  Feet_Per_Second) 
return  Feet  Per_Second_Squared  is 
Temp  ;  Feet_Per_Seconcf; 
begin 

Temp  :=*  Feet_Per_Second  (Left)  *  Right; 
return  Feet_Per_Second_Squared  (Temp); 
end  ~ 


- Distance  *  1/Distance  =*>  Sin_Cos_Ratio 

function  (Left  ;  Feet; 

Right  ;  Inverse_Feet)  return  Trig.Sin_Cos_Ratio  is 

begin 

return  Trig.Sin_Cos_Ratio  (Left  *  Feet  (Right)); 
end 

function  (Left  :  Meters; 

Right  :  Inverse_Meters)  return  Trig.Sin_Cos_Ratio  is 

begin 

return  Trig.Sin_Cos_Ratio  (Left  *  Meters  (Right)); 
end  "*"; 

- Distance  *  1/Distance  ==>  Tan_Ratio 

function  (Left  :  Feet; 

Right  :  Inverse_Feet)  return  Trig.Tan_Ratio  is 

begin 

return  Trig.Tan_Ratio  (Left  *  Feet  (Right)); 
end  "*"? 

function  (Left  :  Meters; 

Right  :  Inverse_Meters)  return  Trig.Tan_Ratio  is 

begin 

return  Trig.Tan_Ratio  (Left  *  Meters  (Right)); 
end 
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—  — Distance  *  INTEGER  ==>  Distance 

function  (Left  :  Feet;  Right  :  INTEGER)  return  Feet  is 
begin 

return  Left  *  Feet  (Right); 
end  "*"; 

function  (Left  :  Meters;  Right  :  INTEGER)  return  Meters  is 
begin 

return  Left  *  Meters  (Right); 
end 

- Distance  *  Tan_Ratio  =>  Distance 

function  (Left  :  Feet; 

Right  :  Trig.Tan_Ratio)  return  Feet  is 

begin 

return  Left  *  Feet  (Right); 
end 

function  (Left  s  Meters; 

Right  s  Trig.Tan_Ratio)  return  Meters  is 

begin 

return  Left  *  Meters  (Right); 
end 

- Distance  *  Sin_Cos_Ratio  *»>  Distance 

function  (Left  :  Feet;  Right  :  Trig.Sin_Cos_Ratio)  return  Feet  is 
begin 

return  Left  *  Feet  (Right); 
end 

function  (Left  :  Meters;  Right  :  Trig.Sin_Cos_Ratio)  return  Meters  is 
begin 

return  Left  *  Meters  (Right); 
end 

- Distance  *  Velocity  =*>  Tan_Ratio; 

function  (Left  :  Feet; 

Right  i  Feet_Per_Second)  return  Trig.Tan_Ratio  is 
Temp  :  Feet; 
begin 

Temp  :=  Left  *  Feet  (Right); 
return  Trig.Tan_Ratio  (Temp); 
end 

function  (Left  :  Meters; 

Right  :  Meters_Per_Second)  return  Trig.Tan_Ratio  is 
Temp  :  Meters; 
begin 

Temp  :=  Left  *  Meters  (Right); 
return  Trig.Tan_Ratio  (Temp); 
end 
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—  — INTEGER  *  Sin  Cos  Ratio  ==>  Sin  Cos  Ratio 


function  (Left  :  INTEGER; 

Right  :  Trig.Sin_Cos_Ratio)  return  Trig.Sin_Cos_Ratio  is 

begin 

return  Trig.Sin_Cos_Ratio  (Left)  *  Right; 
end 


—  — Inverse  Distances  *  Tan  Ratio  ==>  Inverse  Distances 


function  (Left  :  Inverse_Feet; 

Right  :  Trig.Tan_Ratio)  return  Inverse_Feet  is 

begin 

return  Left  *  Inverse_Feet(Right); 
end 


function  (Left  :  Inverse_Meters; 

Right  :  Trig.TanJRatio)  return  Inverse_Meters  is 

begin 

return  Left  *  Inverse_Meters (Right ) ; 
end 


- Sin_Cos_Ratio  *  Angular  Velocity  ==>  Angular  Velocity 


function  (Left  :  Trig.Sin_Cos_Ratio; 

Right  j  Radians_Per_Second) 

•  return  Radians_Per_Second  is 

begin 

return  Radians_Per_Second  (Left)  *  Right; 
end  ~  ~ 


- Sin  Cos  Ratio  *  Distance  ==>  Distai.ee 


function  (Left  :  Trig.Sin_Cos_Ratio;  Right 
begin 

return  Feet  (Left)  *  Right; 
end 


Feet)  return  Feet  is 


function  (Left  :  Trig.Sin_Cos_Ratio;  Right 
begin 

return  Meters  (Left)  *  Right; 

end 


Meters)  return  Meters  is 


— Sin  Cos  Ratio  *  Tan  Ratio  =>  Tan  Ratio 
function  (Left  s  Trig.Sin_Cos_Ratio; 

Right  :  Trig.TanJRatio) 
return  Trig.Tan_Ratio  is 

begin 

return  Trig.Tan_Ratio  (Left)  *  Right; 
end 


- Sin_Cos_Ratio  *  Velocity  ==>  Velocity 


function  (Left  :  Trig.Sin_Cos_Ratio; 

Right  s  Feet_Per_Second)  return  Feet_Per_Second  is 


ruj,t£m 
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begin 

return  Feet_Per_Second  (Left)  *  Right; 
end 

function  (Left  :  Tfig.Sin_Cos_Ratio; 

Right  :  Meters_Per_Second)  return  Meters_Per_Second  is 

begin 

return  Meters_Per_Second  (Left)  *  Right; 
end  ; 


- Tan  Ratio  *  Sin  Cos  Ratio  =>  Tan  Ratio 

function  (Left  :  Trig.Tan_Ratio; 

Right  :  Trig.Sin_Cos_Ratio) 
return  Trig.Tan_Ratio  is 

begin 

return  Left  *  Trig.Tan_Ratio  (Right); 
end 


- Velocity  *  Inverse_Distances  ==>  Angular_Velocity 

function  (Left  :  Feet_per_Second; 

Right  :  Inverse_Feet)  return  Radians_per_Second  is 

‘  begin 

return  Radians  per_Second(Lef t)  *  Radians_per  Second(Right); 
end 

function  (Left  s  Meters_per_Second; 

Right  :  Inverse_Meters)  return  Radians_per_Second  is 

begin 

return  Radians_per_Second(Left)  *  Radians_per_Second(Right); 
end 

- Velocity  *  Sin  Cos  Ratio  ==*>  Velocity 

function  (Left  :  Feet_per_Second; 

Right  :  Trig.Sin_Cos_Ratio)  return  Feet_per_Second  is 

begin 

return  Left  *  Feet_per_Second  (Right); 
end  "*n; 

function  (Left  :  Meters  per_Second; 

Right  s  Trig.Sin_Cos_Ratio)  return  Meters_per_Second  is 

begin 

return  Left  *  Meters_per_Second  (Right); 
end 

- Velocity  *  Velocity  ==>  Velocity  Squared 

function  (Left  :  Feet_per_Second; 

Right  :  Feet_per_Second) 
return  Feet  Squared_per_Second_Squared  is 
Temp  :  Feet_per_Secon3; 
begin 

Temp  :=  Left  *  Right; 

return  Feet_Squared_per_Second_Squared  (Temp); 
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end 

function  (Left  :  Meters_per_Second; 

Right  :  Meters_per_Second) 
return  Meters_Squared_per_Second_Squared  is 
Temp  :  Meters_per_Second; 
begin 

Temp  :=  Left  *  Right; 

return  Meters_Squared_per_Second._Squared  (Temp); 
end 

—  — Velocity  *  Interval  ==>  Distance 

function  (Left  :  Feet_per_Second;  Right  :  Seconds)  return  Feet  is 
Temp  :  Feet_per_Second; 
begin 

Temp  :=  Left  *  Feet_per_Second  (Right); 
return  Feet  (Temp); 
end 

function  (Left  :  Meters_per_Second; 

Right  :  Seconds)  return  Meters  is 
Temp  :  Meters_per_Second; 
begin 

Temp  :=  Left  *  Meters_per_Second  (Right); 
return  Meters  (Temp); 
end 


- Times  Operators  to  Support  DCM  TLCSC 

function  (Left  5  Radians_Per_Second; 

Right  :  Trig.Sin_Cos_Ratio)  return  Radians_Per_Second  is 

begin 

return  Left  *  Radians_Per_Second  (Right); 
end  ; 

function  (Left  :  Radians_Per_Second; 

Right  :  Seconds)  return  Trig. Sin_Cos_Eatio  is 

begin 

return  Trig.Sin_Cos_Ratio  (Seconds  (Left)  *  Right); 
end  ; 

function  (Left  :  Trig.Sin_Cos_Ratio; 

Right  :  Trig.Sin_Cos_Ratio)  return  Trig. Radians  is 
Temp  :  Trig.Sin_Cos_Ratio; 
begin 

Temp  :=  Left  *  Right; 
return  Trig. Radians  (Temp); 
end 

function  (Left  :  Trig.Sin_Cos_Ratio; 

Right  :  Trig. Radians)  return  Trig. Sin_Cos_Ratio  is 

begin 

return  Left  *  Trig.Sin_Cos_Ratio  (Right); 
end 
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—  — Acceleration  /  Velocity  ==>  Angular  Rate 

function  "/"  (Left  :  Feet_per_Second_Squared; 

Right  :  Feet_per_Second)  return  Radians_per_Second  is 

begin 

return  Radians_per_Second  (Left  /  Feet_per_Second_Squared  (Right)); 
end 

function  "/”  (Left  :  Meters_per_Second_Squared; 

Right  :  Meters_per_Second)  return  Radians_per_Second  is 

begin 

return  Radians_per_Second  (Left  /  Meters_per_Second_Squared  (Right)); 
end  "/"; 

function  "/"  (Left  :  Gees; 

Right  :  Feet_per_Second)  return  Radians_per_Second  is 

begin 

return  Radians_per_Second  ((CF.Feet_per_Sec2_per_Gee  *  Left) 

/  Gees  (Right)); 

end  "/" ; 

function  "/"  (Left  :  Gees; 

Right  :  Meters_per_Second)  return  Radians_per_Second  is 

begin 

return  Radians_per_Second  (Gees  (CF.Meters_per_Sec2_per_Gee)  *  Left 

/  Gees  (Right)); 

end  "/"; 

function  "/"  (Left  :  Feet_per_Second_Squared; 

Right  :  Feet  per_SecondJ  return  Degrees_per_Second  is 
Answer  :  Feet_per_Secon3_Squared; 
begin 

Answer  :=  Left  /  Feet_per_Second_Squared  (Right) 

*  Feet_per_Second_Squared  (CF.Degrees_per_Radian) ; 
return  Degrees_per_Second  (Answer); 

end  "/"; 

function  "/'•  (Left  :  Meters_per_Second_Squared; 

Right  :  Meters  per_Second)  return  Degrees_per_Second  is 
Answer  :  Meters_per_Seconcf_Squared; 
begin 

Answer  :=  Left  /  Meters_per_Second_Squared  (Right) 

*  Meters_per_Second_Squared  (CF.Degrees_per_Radian) ; 
return  Degrees_per_Second  (Answer); 

end  "/"; 

function  "/"  (Left  :  Gees; 

Right  :  Feet_per_Second)  return  Degrees_per_Second  is 
Answer  :  Gees; 
begin 

Answer  :=  Gees  (CF.Feet_per_Sec2_per_Gee)  *  Left 

/  Gees  (Right)  *  Gees  (CF.Degrees_per_Radian) ; 
return  Degrees_per_Second  (Answer); 
end  "/"; 

function  "/"  (Left  :  Gees; 
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Right  :  Meters_per_Second)  return  Degrees_per_Second  is 
Answer  :  Gees; 
begin 

Answer  :=  Gees(CF.Meters_per_Sec2_per_Gee)  *  Left 

/  Gees  (Right)  *  Gees  (CF.Degrees_per_Radian) ; 
return  Degrees_per_Second  (Answer); 
end  "/"; 


- Angle  /  Seconds  ==>  Angular  Rate 

function  "/"  (Left  :  Trig. Radians; 

Right  :  Seconds) 
return  Radians_per_Second  is 

begin 

return  Radians_Per_Second  (Seconds  (Left)  /  Right); 
end  M/M; 

function  "/"  (Left  :  Trig. Degrees; 

Right  s  Seconds) 
return  Degrees_Per_Second  is 

begin 

return  Degrees_Per_Second  (Seconds  (Left)  /  Right); 
end  "/"; 

function  ”/”  (Left  :  Trig. Semicircles; 

Right  s  Seconds) 
return  Semicircles_Per_Second  is 

begin 

return  Semicircles_Per_Second  (Seconds  (Left)  /  Right); 
end  "/"; 


- Distance  /  Sin  Cos  Ratio  =*>  Distance 

function  "/"  (Left  :  Feet;  Right  :  Trig.Sin_Cos_Ratio)  return  Feet  is 
begin 

return  Left  /  Feet  (Right); 
end  "/"; 

function  "/"  (Left  :  Meters;  Right  :  Trig.Sin_Cos_Ratio)  return  Meters  is 
begin 

return  Left  /  Meters  (Right); 
end  "/”; 

—  — Distance  /  Tan  Ratio  ==>  Distance 

function  "/"  (Left  s  Feet;  Right  :  Trig.Tan_Ratio)  return  Feet  is 
begin 

return  Left  /  Feet  (Right); 
end  "/"; 

function  "/"  (Left  :  Meters;  Right  :  Trig.Tan_Ratio)  return  Meters  is 
begin 

return  Left  /  Meters  (Right); 
end  "/"; 
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—  — Distance  /  Distance  ==>  Sin_Cos_Ratio 

function  "/"  (Left  :  Feet; 

Right  :  Feet)  return  Trig. Sin_Cos_Ratio  is 
Temp  :  Feet; 
begin 

Temp  :=  Left  /  Right; 
return  Trig.Sin_Cos_Ratio  (Temp); 
end  "/"; 

function  "/"  (Left  :  Meters; 

Right  :  Meters)  return  Trig.Sin_Cos_Ratio  is 
Temp  :  Meters; 
begin 

Temp  :=  Left  /  Right; 
return  Trig.Sin_Cos  Ratio  (Temp); 
end  "/"; 


- Tan_Ratio  :=  Sin_Cos_Ratio  /  Sin_Cos_Ratio 

function  "/"  (Left  :  Trig.Sin_Cos_Ratio; 

Right  •:  Trig.Sin_Cos_Ratio)  return 
Trig.Tan_Ratio  is 
begin  “ 

return  Trig.TanRatio  (Left)  /  Trig. Tan  Ratio  (Right); 
end 

- Tan  Ratio  :*  Sin_Cos_Ratio  /  Tan_Ratio 

function  "/”  (Left  s  Ti:ig.Sin_Cos  Ratio; 

Right  ;  Trig.TanJRatlo)  return  Trig.Tan_Ratio  is 

begin 

return  Trig.Tan_Ratio  (Left)  /  Right; 
end  "/"; 


- Velocity  /  Velocity  ==>  Sin_Cos_Ratio 

function  "/"  (Left  :  Feet_per_Second; 

Right  :  Feet~per_Second)  return  Trig.Sin_Cos_Ratio  is 
Temp  :  Feet_per_Second; 
begin 

Temp  :=  Left  /  Right; 
return  Trig.Sin_Cos_Ratio  (Temp); 
end  "/"; 

function  "/"  (Left  :  Meters_per_Second; 

Right  :  Meters_per_Second) 
return  Trig.Sin_Cos_Ratio  is 
Temp  :  Meters_per_Second; 
begin 

Temp  :=  Left  /  Right; 
return  Trig.Sin_Cos_Ratio  (Temp); 
end  "/"; 

—  — Velocity  /  Velocity  ==>  Tan  Ratio 
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function  "/"  (Left  :  Feet_per_Second; 

Right  :  Feet_Per_Second)  return  Trig.Tan_Ratio  is 
Temp  :  Feet_Per_Second; 
begin 

Temp  :=  Left  /  Right; 
return  Trig.Tan_Ratio  (Temp); 
end  "/"; 

function  '•/"  (Left  :  Meters_per_Second; 

Right  :  Meters_Per_Second)  return  Trig.Tan_Ratio  is 
Temp  :  Meters_Per_Second; 
begin 

Temp  :=  Left  /  Right; 
return  Trig.Tan_Ratio  (Temp); 
end  "/"; 

—  — Velocity  /  Time  ==>  Acceleration 
function  "/"  (Left  :  Feet_per_Second; 

Right  :  Seconds) 

return  Feet_Per_Second_Squared  is 

begin 

return  Feet_Per_Second_Squared  (Seconds  (Left)  /  Right); 
end  "/"; 

function  "/"  (Left  :  Meters_per_Second; 

Right  s  Seconds)  ~ 
return  Meters_Per_Second_Squared  is 

begin 

return  Meters_Per_Second  Squared  (Seconds  (Left)  /  Right); 
end  "/"; 

- Velocity  /  Distance  ==>  Angular  Rate 

function  "/"  (Left  :  Feet_per_Second; 

Right  :  Feet) 

return  Radians_per_Second  is 
Temp  :  Feet; 
begin 

Temp  :=  Feet (Left)  /  Right; 
return  Radians_Per_Second  (Temp); 
end  "/"; 

function  "/”  (Left  ;  Meters_per_Second; 

Right  :  Meters) 
return  Radians_per_Second  is 
Temp  :  Meters; 
begin 

Temp  :=  Meters(Left)  /  Right; 
return  Radians_Per  Second  (Temp); 
end 


- Tan_Ratio  :=  Tan_Ratio  +  Sin_Cos_Ratio 

function  "+"  (Left  :  Trig.Tan_Ratio; 

Right  :  Trig.Sin_Cos  Ratio)  return  Trig. Tan  Ratio  is 
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begin 

return  Left  +  Trig.Tan_Ratio  (Right); 

end 

end  Basic  Data  Types; 


3. 3. 1.1. 7  UTILIZATION  OF  OTHER  ELEMENTS 

The  following  library  units  are  with'd  by  this  part: 

1.  SYSTEM  (a  predefined  part  of  the  Ada  environment) 

2.  Standard_Trig  (P689) 

3.  Conversion  Factors  (P614) 


3. 3. 1.1. 8  LIMITATIONS 
None. 


3. 3. 1.1. 9  LLCSC  DESIGN 
None. 


3.3.1.1.10  UNIT  DESIGN 
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with  Conversion_Factors; 
package  body  Basic_Data_Types  is 


package  Cf  renames  Conversion_Factors; 
use  Trig; 


—  — Define  Operators  for  Basic  Data  Types- 


—  — Acceleration  *  Interval  =  =  >  Velocity 


function  (Left  :  Feet  Per_Second_Squared; 

Right  :  Seconds) 
return  Feet  Per_Second  is 
Temp  :  Feet_Per_Seconcf; 
begin 

Temp  :=  Feet_Per_Second  (Seconds  (Left)  *  Right); 
return  Temp; 

end 


function  "*"  (Left  s  Meters_Per_Second_Squared; 

Right  :  Seconds) 
return  Meters  Per_Second  is 
Temp  :  Meters_Per_Second; 
begin 

Temp  ;=  Meters_Per_Second  (Seconds  (Left)  *  Right); 
return  Temp; 

end 


- Acceleration  *  Sin  Cos  Ratio  =  =  >  Acceleration 


function  (Left  :  Feet_Per_Second_Squared; 

Right  :  Trig.Sin_Cos_Ratio)  return  Feet_Per_Second_Squared  is 

begin 

return  Left  *  Feet_Per_Second_Squared  (Right); 
end 


function  (Left  :  Meters  Per_Second_Squared; 
Right  :  Trig.Sln_Cos_Ratio) 
return  Meters_Per_Second_Squared  is 

begin 

return  Left  *  Meters_Per_Second_Squared  (Right); 
end 


function  (Left  :  Gees; 

Right  :  Trig.Sin_Cos_Ratio)  return  Gees  is 

begin 

return  Left  *  Gees  (Right); 
end  ; 


—  — Acceleration  *  Tan  Ratio  =  =  >  Acceleration 


function  (Left  s  Feet_Per_Second_Squared; 

Right  :  Trig.Tan_Ratio)  return  Feet_Per_Second_Squared  is 

begin 

return  Left  *  Feet_Per_Second_Squared  (Right); 


$ 
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end  ; 


function  (Left  :  Meters_Per_Second_Squared; 

Right  :  Trig.Tan_Ratio)  return  Meters_Per_Second_Squared  is 

begin 

return  Left  *  Meters_Per_Second_Squared  (Right); 
end  ; 


function  (Left  :  Gees;  Right  :  Trig.Tan_Ratio)  return  Gees  is 
begin 

return  Left  *  Gees  (Right); 
end 


—  — AngularVelocity  *  Angular  Velocity  =  =  >  Angular  Velocity  Squared 


function  ,,*M  (Left  :  Radians_Per_Second; 

Right  s  Radians_Per_Second) 
return  Radians_Squared_Per_Second_Squared  is 
Temp  :  Radians_Per_Second; 
begin 

Temp  :=  Left  *  Right; 

return  Radians_Squared_Per_Second_Squared  (Temp); 
end 


function  (Left  :  DegreesPerSecond; 

Right  :  Degrees_Per_Second) 
return  Degrees  SquaredJPer_Second_Squared  is 
Temp  :  Degrees_Per_Secon3; 
begin 

Temp  :=*  Left  *  Right; 

return  Degrees_Squared_Per_Second_Squared  (Temp); 
end  ; 


function  (Left  :  Semicircles_Per_Second; 

Right  s  Semicircles~Per_Second) 
return  Semicircles  Squared_Per_Second_Squared  is 
Temp  :  Semicircles_Per_SeconcI; 
begin 

Temp  :=*  Left  *  Right; 

return  Semicircles_Squared_Per_Second_Squared  (Temp) ; 
end  ; 


—  — Angular  Velocity  /  Sin  Cos  Ratio  =  =  >  Angular  Velocity 


function  "/"  (Left  :  Radians_Per_Second; 

Right  :  Trig.Sin_Cos_Ratio) 
return  Radians_Per_Second  is 

begin 

return  Left  /  Radians_Per_Second  (Right); 
end  "/"; 


—  — Angular  Velocity  *  Tan  Ratio  =  =  >  Angular  Velocity 


function  (Left  :  Radians_Per_Second; 

Right  :  Trig.Tan_Ratio) 
return  Radians  Per  Second  is 


mmmmmmmmmmmmm&mmmimmmsmamm 
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begin 

return  Left  *  Radians_Per_Second  (Right); 

end 

—  — Angular  Velocity  *  Time  Interval  =  =  >  Angle 

function  (Left  :  Degrees_Per_Second; 

Right  :  Seconds)  return  Trig. Degrees  is 

begin 

return  Trig. Degrees  (Right  *  Seconds  (Left)); 
end 

function  (Left  :  Radians_Per_Second; 

Right  :  Seconds)  return  Trig. Radians  is 

begin 

return  Trig. Radians  (Right  *  Seconds  (Left)); 

end 

—  — Angular  Velocity  *  Time  Interval  =  =  >  Earth  Position 

function  (Left  :  Radians_Per_Second; 

Right  :  Seconds) 

return  Earth_Position_Radians  is’ 

begin 

return  Earth_Position  Radians  (Seconds  (Left)  *  Right); 
end  ; 

—  — Angular  Velocity  *  Velocity  =  =  >  Acceleration 

function  (Left  :  RadiansPerSecond; 

Right  s  Keters_Per_Second) 
return  Meters  Per_Second_Squared  is 
Temp  :  Meters_Per_SeconcT; 
begin 

Temp  :=  Meters_Per_Second  (Left)  *  Right; 
return  Meters_Per_Second_Squared  (Temp); 
end  ;  * 

function  (Left  :  Radians_Per_Second; 

Right  ;  Feet_Per_Second) 
return  Feet  Per_Second_Squared  is 
Temp  s  Feet_Per_Secon<I; 
begin 

Temp  :=  Feet_Per_Second  (Left)  *  Right; 
return  Feet_Per_Second  Squared  (Temp); 
end 


—  — Distance  *  I /Distance  =  =  >  Sin  Cos  Ratio 

function  (Left  :  Feet; 

Right  :  Inverse_Feet)  return  Trig.Sin_Cos_Ratio  is 

begin 

return  Trig.Sin_Cos_Ratio  (Left  *  Feet  (Right)); 
end  "*M ; 


function  (Left  s  Meters; 
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Right  :  Inverse_Meters)  return  Trig.Sin_Cos_Ratio  is 

begin 

return  Trig.Sin_Cos_Ratio  (Left  *  Meters  (Right)); 
end  ***** ; 

—  — Distance  *  I  /Distance  =  =  >  Tan  Ratio 

function  (Left  :  Feet; 

Right  :  Inverse_Feet)  return  Trig.Tan_Ratio  is 

begin 

return  Trig.Tan_Ratio  (Left  *  Feet  (Right)); 

end 

function  (Left  :  Meters; 

Right  :  Inverse_Meters)  return  Trig.Tan_Ratio  is 

begin 

return  Trig.Tan_Ratio  (Left  *  Meters  (Right)); 
end 

—  — Distance  *  INTEGER  =  =  >  Distance 


i 


function  "*"  (Left  :  Feet;  Right  s  INTEGER)  return  Feet  is 
begin 

return  Left  *  Feet  (Right); 
end  "*" ; 


function  (Left  :  Meters;  Right  :  INTEGER)  return  Meters  is 
begin 

return  Left  *  Meters  (Right); 

end 


—  — Distance  *  Tan  Ratio  =  >  Distance 


function  (Left  :  Feet; 

Right  :  Trig.Tan_Ratio)  return  Feet  is 

begin 

return  Left  *  Feet  (Right); 
end 


function  (Left  :  Meters; 

Right  :  Trig.Tan_Ratio)  return  Meters  is 

begin 

return  Left  *  Meters  (Right); 
end 


—  — Distance  *  Sin  Cos  Ratio  =  =  >  Distance 


function  (Left  :  Feet;  Right  :  Trig.Sin_Cos_Rati°)  return  Feet  is 
begin 

return  Left  *  Feet  (Right); 
end 


function  (Left  :  Maters;  Right  :  Trig.Sin_Cos_Ratio)  return  Meters  is 
begin 

return  Left  *  Meters  (Right); 
end 


'-*'1 
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—  —  Distance  *  Velocity  =  >  Tan  Ratio; 

function  (Left  :  Feet; 

Right  :  Feet_Per_Second)  return  Trig.Tan_Ratio  is 
Temp  :  Feet; 
begin 

Temp  :=  Left  *  Feet  (Right); 
return  Trig.Tan_Ratio  (Temp); 

and 

function  (Left  :  Meters; 

Right  s  Meters_Per_Second)  return  Trig.Tan_Ratio  is 
Temp  :  Meters; 
begin 

Temp  t-  Left  *  Meters  (Right); 
return  Trig.Tan_Ratio  (Temp); 

end 


—  — INTEGER  *  Sin  Cos  Ratio  =  =  >  Sin  Cos  Ratio 

function  (Left  :  INTEGER; 

Right  :  Trig.Sin_Cos_Ratio)  return  Trig.Sin_Cos_Ratio  is 

begin 

return  Trig.Sin_Cos_Ratio  (Left)  *  Right; 
end 


—  — Inverse  Distances  *  Tan  Ratio  =  =  >  Inverse  Distances 

function  (Left  s  Inverse_Feet; 

Right  :  Trig.Tan_Ratio)  return  Inverse_Feet  is 

begin 

return  Left  *  Inverse_Feet (Right); 

end 

function  (Left  :  Inverse_Meters; 

Right  :  Trig.Tan_Ratio)  return  Inverse_Meters  is 

begin 

return  Left  *  Inverse  Meters(Right); 
end  "*"5 


—  — Sin  Cos  Ratio  *  Angular  Velocity  =  =  >  Angular  Velocity 

function  (Left  :  Trig.Sin_Cos_Ratio; 

Right  :  Radians_Per_Second) 
return  Radians_Per_Second  is 

begin 

return  Radians_Per_Second  (Left)  *  Right; 
end  "*"} 

—  — Sin  Cos  Ratio  *  Distance  =  =  >  Distance 

function  (Left  ;  Trig.Sin_Cos_Ratio;  Right  :  Feet)  return  Feet  is 
begin 

return  Feet  (Left)  *  Right; 
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end 

function  (Left  :  Trig.Sin_Cos_Ratio;  Right  :  Meters)  return  Meters  is 
begin 

return  Meters  (Left)  *  Right; 
end  "*" ; 

—  — Sin  Cos  Ratio  *  Tan  Ratio  =  >  Tan  Ratio 

function  (Left  :  Trig.Sin_Cos_Ratio; 

Right  :  Trig.Tan_Ratio) 
return  Trig.Tan_Ratio  is 

begin 

return  Trig.Tan_Ratio  (Left)  *  Right; 
end  "*'* ; 

—  — Sin  Cos  Ratio  *  Velocity  =  =  >  Velocity 

function  (Left  :  Trig.Sin_Cos_Ratio; 

Right  :  Feet_Per~Second)  return  Feet_Per_Second  is 

begin 

return  Feet_Per_Second  (Left)  *  Right; 
end 

function  "*"  (Left  s  Trig.Sin_Cos_Ratio; 

Right  j  Meters_Per_Second)  return  Meter's_Per_Second  is 

begin 

return  MetersPerSecond  (Left)  *  Right; 
end  ~  - 


—  —  Tan  Ratio  *  Sin  Cos  Ratio  -  >  Tan  Ratio 

function  "*"  (Left  j  Trig.Tan_Ratio; 

Right  :  Trig.Sin~Cos_Ratio) 
return  Trig.Tan_Ratio  is 

begin 

return  Left  *  Trig.Tan_Ratio  (Right); 
end  ; 


—  —  Velocity  *  Inverse  Distances  =  =  >  AngularVelocity 

function  (Left  :  Feet_Per_Second; 

Right  :  Inverse_Feet)  return  Radians_Per_Second  is 

begin 

return  Radians_Per_Second(Left)  *  Radians_Per_Second(Right) ; 

end 

function  (Left  :  Meters_Per_Second; 

Right  :  Inverse_Meters)  return  Radians_Per_Second  is 

begin 

return  Radians_Per_Second(Lef t)  *  Radians_Per_Second(Right) ; 
end  "*"; 

—  —  Velocity  *  Sin  Cos  Ratio  =  =  >  Velocity 

function  (Left  :  Feet_Per_Second; 

Right  :  Trig.Sin~Cos_Ratio)  return  Feet_Per_Second  is 
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begin 

return  Left  *  Feet_Per_Second  (Right); 

end 

function  (Left  :  Meters_Per_Second; 

Right  :  Trig.Sin_Cos_JRatio)  return  Meters_Per_Second  is 

begin 

return  Left  *  Meters_Per_Second  (Right); 

end 

—  —  Velocity  *  Velocity  -  =  >  Velocity  Squared 

function  '**"  (Left  :  Feet_Per_Second; 

Right  :  Feet_Per_Second) 
return  Feet  Squared_Per_Second_Squared  is 
Temp  :  Feet_Per_SecomT; 
begin 

Temp  :=  Left  *  Right; 

return  Feet_Squared_Per_Second_Squared  (Temp) ; 

end 

function  (Left  :  Meters_Per_Second; 

Right  s  Meters_Per_Second) 
return  Meters  Squared_Per_Second_Squared  is 
Temp  s  Me  t ers_Per_Seconc[ ; 
begin 

Temp  Left  *  Right; 

return  Meters_Squared_Per_Second  Squared  (Temp); 

end 

—  —  Velocity  *  Interval  =  =  >  Distance 

function  (Left  ;  Feet_Per_Second;  Right  :  Seconds)  return  Feet  is 
Temp  :  Feet_Per_Second; 
begin 

Temp  :=  Left  *  Feet_Per_Second  (Right); 
return  Feet  (Temp); 

end 

function  "*"  (Left  :  Meters_Per_Second; 

Right  :  Seconds)  return  Meters  is 
Temp  :  Meters_Per_Second; 
begin 

Temp  :=  Left  *  Meters_Per_Second  (Right); 
return  Meters  (Temp); 
end  "*"; 


—  —  Times  Operators  to  Support  DCM  TLCSC 

function  (Left  :  Radians_Per_Second; 

Right  :  Trig.Sin_Cos_Ratio)  return  Radians_Per_Second  is 

begin 

return  Left  *  Radians_Per_Second  (Right); 
end 

function  (Left  :  Radians  Per  Second; 
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Right  :  Seconds)  return  Trig.Sin_Cos_Ratio  is 

begin 

return  Trig. Sin_Cos_Ratio  (Seconds  (Left)  *  Right); 

end 

function  (Left  :  Trig.Sin_Cos_Ratio; 

Right  :  Trig.Sin_Cos_Ratio)  return  Trig. Radians  is 
Temp  :  Trig.Sin_Cos_Ratio; 
begin 

Temp  :=  Left  *  Right; 
return  Trig. Radians  (Temp); 
end  "*"; 

function  (Left  :  Trig. Sin  Cos__Ratio; 

Right  :  Trig. Radians)  return  Trig.Sin_Cos_Ratio  is 

begin 

return  Left  *  Trig.Sin_Cos_Ratio  (Right); 
end 


—  — Acceleration  /  Velocity  =  =  >  Angular  Rate 

function  "/"  (Left  :  Feet_Per_Second_Squared; 

Right  s  Feet_Per_Second)  return  Radians_Per_Second  is 

begin 

return  Rad i ans_Pe r_S e cond  (Left  /  Feet_Per  Second_Squared  (Right))? 
end  "/"; 

function  "/"  (Left  :  MetersPerSecondSquared; 

Right  j  Meters_Per_SecondJ  return  Radians_Per_Second  is 

begin 

return  Radians_Per_Second  (Left  /  MetersPerSecondSquared  (Right)); 
end  "/"; 

function  "/"  (Left  s  Gees; 

Right  :  Feet_Per_Second)  return  Radians_Per_Second  is 

begin 

return  Radians_Per_Second  ((Cf .Feet_Per_Sec2_Per_Gee  *  Left) 

/  Gees  (Right)); 

end  "/"; 

function  ”/"  (Left  :  Gees; 

Right  :  Meters_Per_Second)  return  Radians_Per_Second  is 

begin 

return  Radians_Per_Second  (Gees  (Cf .Meters_Per_Sec2_Per_Gee)  *  Left 

/  Gees  (Right ))7 

end  "/"; 

function  "/"  (Left  :  Feet_Per_Second  Squared; 

Right  :  Feet_Per_SecondJ  return  Degrees_Per_Second  is 
Answer  :  Feet_Per_Second_Squared; 
begin 

Answer  :=  Left  /  Feet_Per_Second_Squared  (Right) 

*  Feet_Per_Second_Squared  (Cf .Degrees_Per_Radian) ; 
return  Degrees_Per  Second  (Answer); 
end  "/"; 
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function  "/"  (Left  :  Meters_Per_Second_Squared; 

Right  :  Meters_Per_Second)  return  Degrees_Per_Second  is 
Answer  :  Meters_Per_Second_Squared; 
begin 

Answer  :=  Left  /  Meters_Per_Second_Squared  (Right) 

*  Meters_Per_Second_Squared  (Cf .Degrees_Per_Radian) ; 
return  Degrees_Per_Second  (Answer); 

end 

function  "/"  (Left  :  Gees; 

Right  :  Feet_Per_Second)  return  Degrees_Per_Second  is 
Answer  :  Gees; 
begin 

Answer  :=  Gees  (Cf .Feet_Per_Sec2_Per_Gee)  *  Left 

/  Gees  (Right)  *  Gees  (Cf.Degrees_Per_Radian); 
return  Degrees_Per_Second  (Answer); 
end  "/"; 

function  "/”  (Left  :  Gees; 

Right  :  Meters_Per_Second)  return  Degrees_Per_Second  is 
Answer  :  Gees; 
begin 

Answer  :=  Gees(Cf .Meters_Per_Sec2_Per_Gee)  *  Left 

/  Gees  (Right)  *  Gees  (Cf .Degrees_Per_Radian); 
return  Degrees_Per_Second  (Answer); 

end 


- Angle  /  Seconds  =  =  >  Angular  Rate 

function.  "/"  (Left  :  Trig. Radians; 

Right  :  Seconds) 
return  Radians_Per_Second  is 

begin 

return  Radians_Per_Second  (Seconds  (Left)  /  Right); 
end  "/" ; 

function  "/"  (Left  :  Trig. Degrees; 

Right  :  Seconds) 
return  Degrees_Per_Second  is 


begin 

return  Degrees_Per_Second  (Seconds  (Left)  /  Right); 
end  "/" ; 


function  "/"  (Left  :  Trig. Semicircles; 

Right  :  Seconds) 

return  Semicircles_Per_Second  is 

begin 

return  Semicircles_Per_Second  (Seconds  (Left)  /  Right); 
end  "/" ; 


- Distance  /  Sin  Cos  Ratio  =  =  >  Distance 


function  "/"  (Left  :  Feet;  Right  :  Trig.Sin_Cos_Ratio)  return  Feet  is 
begin 

return  Left  /  Feet  (Right); 


& 
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end  "/"; 

function  "/"  (Left  :  Meters;  Right  :  Trig.Sin_Cos_Ratio)  return  Meters  is 
begin 

return  Left  /  Meters  (Right); 
end  "/"; 

—  — Distance  /  Tan  Ratio  =  =  >  Distance 

function  "/"  (Left  :  Feet;  Right  :  Trig.Tan_Ratio)  return  Feet  is 
begin 

return  Left  /  Feet  (Right); 

end 

function  "/"  (Left  :  Meters;  Right  :  Trig.Tan_Ratio)  return  Meters  is 
begin 

return  Left  /  Meters  (Right); 
end  "/"; 

—  —  Distance  /  Distance  =  =  >  Sin  Cos  Ratio 

function  "  /"  (Left  :  Feet; 

Right  s  Feet)  return  Trig.Sin_Cos_Ratio  is 
Temp  :  Feet; 
begin 

Temp  :=*  Left  /  Right; 
return  Trig.Sin_Cos_Ratio  (Temp); 
end  "/"; 

function  "/"  (Left  :  Meters; 

Right  :  Meters)  return  Trig.Sin_Cos_Ratio  is 
Temp  :  Meters; 
begin 

Temp  :=  Left  /  Right; 
return  Trig.Sin_Cos_Ratio  (Temp); 
end  "/"; 


—  — Tan  Ratio  :=  Sin  Cos  Ratio  /  Sin  Cos  Ratio 

function  "/"  (Left  :  Trig. Sin_Cos_Ratio; 

Right  j  Trig.Sin_Cos_Ratio)  return 
Trig.Tan_Ratio  is 

begin 

return  Trig.Tan_Ratio  (Left)  /  Trig. Tam  Ratio  (Right); 
end  "/"; 

—  — Tan_Ratio  ;=  Sin  Cos  Ratio  /  Tan  Ratio 

function  "/"  (Left  :  Trig. Sin_Cos_Ratio; 

Right  :  Trig.Tan_Ratio)  return  Trig.Tan_Ratio  is 

begin 

return  Trig. Tan  Ratio  (Left)  /  Right; 


—  —  Velocity  /  Velocity  =  =  >  Sin  Cos  Ratio 
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function  "/"  (Left  :  Feet_Per_Second; 

Right  :  Feet_Per_Second)  return  Trig.Sin_Cos_Ratio  is 
Temp  :  Feet_Per_Second; 
begin 

Temp  :=  Left  /  Right; 
return  Trig.Sin_Cos_Ratio  (Temp); 
end  "/"; 

function  "/"  (Left  :  Meters_Per_Second; 

Right  :  Meters_Per_Second) 
return  Trig.Sin_Cos_Ratio  is 
Temp  :  Meters_Per_Second; 
begin 

Temp  :=  Left  /  Right; 

return  Trig.Sin_Cos_Ratio  (Temp); 

end 

—  —  Velocity  /  Velocity  =  =  >  Tan  Ratio 

function  "/"  (Left  :  Feet_Per_Second; 

Right  :  Feet_Per_Second)  return  Trig.Tan_Ratio  is 
Temp  s  Feet_Per_Second; 
begin 

Temp  :=*  Left  /  Right; 
return  Trig.Tan_Ratio  (Temp); 
end  "/"; 

function  "/"  (Left  :  Meters_Per_Second; 

Right  :  Meters_Per_Second)  return  Trig.Tan  Ratio  is 
Temp  ;  Meters_Per_Second; 
begin 

Temp  :=  Left  /  Right; 
return  Trig.Tan_Ratio  (Temp); 
end  "/"; 

—  —  Velocity  /  Time  =  =  >  Acceleration 
function  "/"  (Left  :  Feet_Per_Second; 

Right  :  Seconds) 

return  Feet_Per_Second_Squared  is 

begin 

return  Feet_Per_Second_Squared  (Seconds  (Left)  /  Right); 
end  "/"; 

function  "/"  (Left  :  Meters_Per_Second; 

Right  :  Seconds) 

return  Meters_Per_Second_Squared  is 

begin 

return  Meters_Per_Second_Squared  (Seconds  (Left)  /  Right); 
end  "/"; 

—  —  Velocity  /  Distance  =  =  >  Angular  Rate 

function  "/"  (Left  :  Feet_Per_Second; 

Right  :  FeetJ  ~ 
return  Radians_Per_Second  is 
Temp  :  Feet; 
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begin 

Temp  :=  Feet(Left)  /  Right; 
return  Radians_Per_Second  (Temp); 
end  "/"; 

function  "/"  (Left  :  Meters_Per_Second; 
Right  :  Meters) 
return  Radians_Per_Second  is 
Temp  :  Meters; 
begin 

Temp  :=  Meters(Left)  /  Right; 
return  Radians_Per_Second  (Temp); 
end  "/"; 


—  — Tan  Ratio  Tan  Ratio  +  Sin  Cos  Ratio 

function  "+"  (Left  :  Trig.Tan_Ratio; 

Right  :  Trig.Sin_Cos_Ratio)  return  Trig.Tan_Ratio  is 

begin 

return  Left  +  Trig.Tan_Ratio  (Right); 
end  ; 

end  Basic  Data  Types; 


■vs 
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3. 3. 1.2  KALMAN  FILTER  DATA  TYPES  (BODY)  TLCSC  P622  (CATALOG  #P158-0) 

This  part,  which  is  designed  as  an  Ada  generic  package,  defines  data  types  (and 
operators  on  those  data  types)  used  in  a  Kalman  Filter. 

The  decomposition  for  this  part  is  the  same  as  that  shown  in  the  Top-Level 
Design  Document. 


3. 3. 1.2.1  REQUIREMENTS  ALLOCATION 
This  part  meets  CAMP  requirement  R023 


3. 3. 1.2. 2  LOCAL  ENTITIES  DESIGN 
None. 

3. 3. 1.2. 3  INPUT/OUTPUT 
GENERIC  PARAMETERS: 

Data  types: 

The  following  table  describes  the  generic  formal  types  required  by  this  part: 


|  Name 

|  Base  Type 

Description  ^  | 

| State  Indices 

|  Discrete 

Indexes  the  matrices  which  are  | 

i  type 

l 

a  function  of  the  _#  of  states j 

1 

(Measurement 

1 

|  Discrete 

1 

Indexes  the  matrices  which  are  | 

|  Indices 

i  type 

a  function  of  the  #  of  j 

1 

| Kalman  Filter 

1 

| floating 

measurements  j 

Elements  making  up  the  j 

!  Elements 

|  point  type 

matrices  of  the  Kalman  j 

1 

Filter  j 

3. 3. 1.2. 4  LOCAL  DATA 
None. 

3. 3. 1.2. 5  PROCESS  CONTROL 
Not  applicable. 


3. 3. 1.2. 6  PROCESSING 

The  following  describes  the  processing  performed  by  this  part: 
package  body  Kalman_Filter_Data_Types  is 
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- NAME:  Active  H  Vector 


function  Active_H_Vector  (Source  :  M_By_N_Statically_Sparse. Matrices; 

Row  :  Measurement_Indices) 
return  N_By_l. Vectors  is 
Work_Vector  :  N_By_l. Vectors; 
begin 

for  I  in  State_Indices  loop 

Work_Vector  (I)  :=  Source  (Row,  I); 
end  loop; 

return  Work_Vector; 
end  Active  H  Vector; 


- NAME:  Element 


function  Element  (Source  :  N_By_N_Symmetric. Matrices; 
Row  :  State_Indices: 

Column  :  State  Indices) 
return  Kalman_Fllter_Elements  is 

begin 

return  Source  (Row,  Column); 
end  Element; 


- NAME:  Row 


function  Row  (Source  :  N_By_N_Symmetric. Matrices; 
Row  :  State_Indices) 
return  N_By_l. Vectors  is 
Work_Vector  :  N_By_l. Vectors; 
begin 

for  I  in  State_Indices  loop 

Work_Vector  (I)  :=  Source  (Row, I); 
end  loop; 

return  Work_Vector; 
end  Row; 

end  Kalman_Filter_Data  Types; 


3. 3. 1.2. 7  UTILIZATION  OF  OTHER  ELEMENTS 
UTILIZATION  OF  OTHER  ELEMENTS  IN  TOP  LEVEL  COMPONENT: 

The  following  tables  describe  the  elements  used  by  this  part  but  defined 
elsewhere  in  this  top  level  component: 


Data  types: 
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The  following  table  summarizes  the  types  required  by  this  part  and  defined  in 
ancestral  units: 


Name 


Element  Type 


| Index  Types  |  Matrix  Type  | 


| M  By  N_Statically 
j  Sparse 

I 

| Kalman  Filter  Elements 

1 

[Measurement  | Sparse 
j  _Indices,  j 

j State_Indices  j 

1 

i 

1 

1 

1 

|N_By  N  Diagonal 

1 

| Kalman_Filter_Elements 

l 

1  1 
[ State_Indices, |Di agonal 
j State_Indices  j 

1 

1 

1 

|N  By  N  Symmetric 

1 

| Kalman  Filter  Elements 

i 

1  1 
| State_Indices, | Symmetric 
j State_Indices  j 

I 

1 

1 

|N  By  1  (Vector) 

I 

| Kalman_Filter_Elements 

1  i 

| State  Indices  [Vector 

1 

1 

3.3. 1.2.8  LIMITATIONS 
None. 


3. 3. 1.2. 9  LLCSC  DESIGN 
None. 


3.3.1.2.10  UNIT  DESIGN 
None. 


wSsi 
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package  body  Kalman_Filter__Data_Types  is 


—  —  NAME:  Active  H  Vector 


function  Active_H_Vector  (Source  :  M_By_N_Statically_Sparse. Matrices 

Row  :  Measurement_Indices) 
return  N_By_l. Vectors  is 
Vork_Vector  :  N_By_l. Vectors; 
begin 

for  I  in  State_Indices  loop 

Work_Vector  (I)  :=  Source  (Row,  I); 
end  loop; 

return  Work_Vector; 
end  Active  H  Vector; 


—  — NAME:  Element 


function  Element  (Source  :  N_By_N_Symmetric. Matrices; 
Row  s  Slate_Indices; 

Column  :  State~Indices) 
return  Kalman_Fllter_Elements  is 

begin 

return  Source  (Row,  Column); 
end  Element; 


- NAME:  Row 


function  Row  (Source  :  N_By_N_Symmetric. Matrices; 
Row  :  State_Indices) 
return  N_By_l. Vectors  is 
Work_Vector  :  N_By_l. Vectors; 
begin 

for  I  in  State_Indices  loop 

Work_Vector  (I)  :=  Source  (Row, I); 
end  loop; 

return  Work_Vector; 
end  Row; 

end  Kalman  Filter  Data  Types; 


.  .•vyv"- 
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3.3. 1.3  AUTOPILOT  DATA  TYPES  (BODY)  TLCSC  P623  (CATALOG  #P93-0) 

This  part,  which  is  designed  as  an  Ada  package,  defines  simple  data  types  and 
special  operators  for  use  in  the  Autopilot  Package. 

The  decomposition  for  this  part  is  the  same  as  that  shown  in  the  Top-Level 
Design  Document. 


3. 3. 1.3.1  REQUIREMENTS  ALLOCATION 
This  part  meets  CAMP  requirement  R163. 


3. 3. 1.3. 2  LOCAL  ENTITIES  DESIGN 
None. 

3. 3. 1.3. 3  INPUT/OUTPUT 
None. 

3. 3. 1.3. 4  LOCAL  DATA 
None. 

3. 3. 1.3. 5  PROCESS  CONTROL 
Not  applicable. 


3. 3. 1.3. 6  PROCESSING 

The  following  describes  the  processing  performed  by  this  part: 
package  body  Autopilot_Data_Types  is 


- Define  Operators  for  Autopilot  Data  Types- 


- Acceleration  Feedback  *  Gain  ==>  Fin  Deflections- 


function  (Left  :  Acceleration_Feedbacks_FPS2; 

Right  :  FPS2_to_Degrees_Gain)  - 
return  Fin_Deflections_Degrees  is 

begin 

return  Fin_Deflections  Degrees 

"(Left  *  Acceleration  Feedbacks  FPS2  (Right)); 
end 


5 

2 


* 

5 

i 

j 

> 


i 

i 


H 


i 


EwV 
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function  (Left  :  Acceleration_Feedbacks_MPS2; 

Right  :  MPS2_to_Degrees_Gain) 
return  Fin_Deflections_Degrees  is 

begin 

return  Fin_Deflections_Degrees 

(Left  *  Acceleration_Feedbacks_MPS2  (Right)); 
end 

function  (Left  :  Acceleration_Feedbacks_Gees; 

Right  :  Gees_to_Degrees_Gain) 
return  Fin_Deflections_Degrees  is 

begin 

return  Fin  Deflections_Degrees 

(Lelt  *  Acceleration_Feedbacks_Gees  (Right)); 
end 


function  (Left  :  Acceleration_Feedbacks_FPS2; 

Right  :  FPS2_to_Radians_Gain) 
return  Fin_Deflections_Radians  is 

begin 

return  Fin_Deflections  Radians 

(Left  *  AcceIeration_Feedbacks_FPS2  (Right)); 
end 

function  (Left  :  Acceleration_Feedbacks_MPS2; 

Right  :  MPS2_  to_Radians_Gain) 
return  Fin_Deflections_Radians  is 

begin 

return  Fin  Deflections  Radians 

^Left  *  AcceIeration_Feedbacks_MPS2  (Right)); 

end 

function  (Left  t  Acceleration_Feedbacks_Gees; 

Right  :  Gees_to_Radians_Gain) 
return  Fin_Deflections_Radians  is 

begin 

return  Fin_Deflections  Radians 

(Left  *  AcceIeration_Feedbacks_Gees  (Right)); 
end 


- Acceleration  Commands  *  Gain  ==>  Fin  Deflections- 


function  (Left  :  Acceleration_Commands_FPS2; 

Right  :  FPS2_to_Degrees_Gain) 
return  Fin_Deflections_Degrees  is 

begin 

return  Fin_Deflections_Degrees 

(Left  *  Acceleration_Commands_FPS2  (Right)); 

end  "*"; 

function  (Left  :  Acceleration_Commands_HPS2; 

Right  :  MPS2_to_Degrees_Gain) 
return  Fin  Deflections_Degrees  is 


,\*V\V ;V  .V,VA>  ,v 

*,  w  *  'T,  . 

~  -  “ 
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begin 

return  Fin_Deflections_Degrees 

(Left  *  Acceleration_Commands_MPS2  (Right)); 
end 

function  (Left  :  Acceleration_Commands_Gees; 

Right  s  Gees_to_Degrees_Gain) 
return  Fin_Deflections_Degrees  is 

begin 

return  Fin_Deflections  Degrees 

(Left  *  AcceIeration_Commands_Gees  (Right)); 

end  "*"; 

function  "*"  (Left  :  Acceleration_Commands_FPS2; 

Right  :  FPS2_to_Radians_Gain) 
return  Fin_Deflections_Radians  is 

begin 

return  Fin_Deflections_Radians 

(Left  *  Acceleration_Commands_FPS2  (Right)); 

end 

function  (Left  :  Acceleration_Commands_MPS2; 

Right  :  MPS2_to_Radians_Gain) 
return  Fin_Deflections_Radians  is 

begin 

return  Fin  Deflections  Radians 

^Left  *  AcceIeration_Commands_MPS2  (Right)); 

end  ; 

function  "*"  (Left  :  Acceleration_Commands_Gees; 

Right  :  Gees_to_Radians_Gain) 
return  FinJDeflectionsjRadians  is 

begin 

return  Fin_Deflections  Radians 

(Left  *  AcceIeration_Commands_Gees  (Right)); 
end 


- Feedback  Rates  *  Gain  ==>  Fin  Deflections- 


- Ambiguity  in  the  meaning  of  the  "*"  operator  has  required  changing  the 

- standard  order  for  explicit  type  conversion.  The  bodies  of  the  next  two 

—  — functions  perform  explicit  type  conversion  on  the  "Left"  operand  to  avoid 

- ambiguity  with  subprograms  derived  from  the  type  declarations  of  Feedback 

-  Rates_Degrees  and  Feedback  Rates_Radians. 


function  "*"  (Left  :  Feedback_Rates_Degrees; 

Right  :  DPS_to  Degrees_Gain) 
return  Fin_DefIections_Degrees  is 

begin 

return  Fin_Deflections_Degrees 

(DPS_to_Degrees_Gain  (Left)  *  Right); 
end 


function  fLeft  :  Feedback  Rates  Radians: 


CAMP  Software  Detailed  Design  Document 


Page  48 


return  Fin_Deflections_Radians  is 

begin 

return  Fin_Deflections_Radians 

(RPS_to_Radians_Gain  (Left)  *  Right); 
end 


—  — Roll  Commands  *  Gain  ==>  Angle- 


function  (Left  s  Roll_Commands_Degrees ; 

Right  :  Gain_in_Degrees) 
return  Fin_Deflections_Degrees  is 

begin 

return  Fin_Deflections_Degrees 

(Left  *  Roll_Commands_Degrees  (Right)); 
end 

function  (Left  :  Roll_Commands_Radians; 

Right  s  Gain~in_Radians) 
return  Fin_Deflections_Radians  is 

begin 

return  Fin_Deflections_Radians 

(Left  *  Roll_Commands_Radians  (Right)); 
end 


- Acceleration  Commands  +  Acceleration  Feedbacks  »»>  Acceleration  Commands- 


function  "+"  (Left  :  Acceleration_Commands_FPS2; 

Right  :  Acceleration_Feedbacks_FPS2) 
return  Acceleration_Commands_FPS2  is 

begin 

return  Left  +  Acceleration_Commands_FPS2  (Right); 
end  "+"; 

function  "+"  (Left  :  Acceleration_Commands_MPS2; 

Right  :  Acceleration_Feedbacks_MPS2) 
return  Acceleration_Commands_MPS2  is 

begin 

return  Left  +  Acceleration_Commands_MPS2  (Right); 
end  "+"; 

function  "+"  (Left  :  Acceleration_Commands_Gees; 

Right  :  Acceleration_Feedbacks_Gees) 
return  Acceleration_Comraands_Gees  is 

begin 

return  Left  +  Acceleration_Commands_Gees  (Right); 
end  "+"; 


- Acceleration  Commands  -  Acceleration  Feedbacks  ==>  Acceleration  Commands- 


function  (Left  :  Acceleration_Commands_FPS2; 

Right  :  Acceleration  Feedbacks  FPS2) 
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return  Acceleration_Commands_FPS2  is 

begin 

return  Left  -  Acceleration_Commands_FPS2  (Right); 
end  ; 

function  (Left  :  Acceleration_Commands_MPS2; 

Right  :  Acceleration_Feedbacks_MPS2) 
return  Acceleration_Commands_MPS2  is 

begin 

return  Left  -  Acceleration_Commands_MPS2  (Right); 
end 

function  (Left  :  Acceleration_Commands_Gees; 

Right  :  Acceleration_Feedbacks_Gees) 
return  Acceleration_Commands_Gees  is 

begin 

return  Left  -  Acceleration_Commands_Gees  (Right); 
end 


- Roll  Commands  -  Missile  Attitudes  ==>  Roll  Commands- 


function  (Left  :  Roll  Command s_Degrees; 

Right  s  MissIle_Attitudes_Degrees) 
return  Roll_Commands_Degrees  is 

begin 

return  Left  -  Roll_Commands_Degrees  (Right); 
end 

function  (Left  :  Roll  Commands_Radians; 

Right  s  MissTle_Attitudes_Radians) 
return  Roll_Commands_Radians  is 

begin 

return  Left  -  Roll_Commands_Radians  (Right); 
end 

end  Autopilot_Data_Types; 


3. 3. 1.3. 7  UTILIZATION  OF  OTHER  ELEMENTS 
None. 

3. 3. 1.3. 8  LIMITATIONS 
None. 


3.3. 1.3.9  LLCSC  DESIGN 
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3.3.1.3.10  UNIT  DESIGN 


None. 
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package  body  Autopilot_Data_Types  is 


—  — Define  Operators  for  Autopilot  Data  Types- 


—  — Acceleration  Feedback  *  Gain  =  =  >  FinDeflections- 


function  (Left  :  Acceleration_Feedbacks_Fps2; 

Right  :  Fps2_To_Degrees_Gain) 
return  Fin_Deflections_Degrees  is 

begin 

return  Fin_Deflections  Degrees 

(Left  *  AcceIeration_Feedbacks_Fps2  (Right)); 

end  ; 

function  (Left  :  Acceleration_Feedbacks_Mps2; 

Right  :  Mps2_To_Degrees_Gain) 
return  Fin_Deflections_Degrees  is 

begin 

return  Fin  Deflections  Degrees 

^Left  *  AcceTeration_Feedbacks_Mps2  (Right)); 

end 

function  (Left  :  Acceleration_Feedbacks_Gees; 

Right  :  Gees_To_Degrees_Gain) 
return  Fin_Deflections_Degrees  is 

begin 

return  Fin  Deflections_Degrees 

(Lelt  *  Acceleration_Feedbacks_Gees  (Right)); 

end  ,,*n; 


function  "*'•  (Left  :  Acceleration_Feedbacks_Fps2; 

Right  :  Fps2_To_Radians_Gain) 
return  Fin_Deflections_Radians  is 

begin 

return  Fin  Deflections  Radians 

^Left  *  AcceIeration_Feedbacks_Fps2  (Right)); 
end 

function  (Left  :  Acceleration_Feedbacks_Mps2; 

Right  :  Mps2_To_Radians_Gain) 
return  Fin_Deflections_Radians  is 

begin 

return  Fin_Deflections  Radians 

(Left  *  AcceIeration_Feedbacks_Mps2  (Right)); 
end 

function  (Left  :  Acceleration_Feedbacks_Gees; 

Right  :  Gees_To_Radians_Gain)  - 
return  Fin_Deflections  Radians  is 

begin 
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return  Fin_Deflections_Radians 

(Left  *  Acceleration_Feedbacks_Gees  (Right)); 

end  ; 


—  — Acceleration  Commands  *  Gain  =  =  >  Fin  Dejlections- 


function  (Left  :  Acceleration_Commands_Fps2; 

Right  i  Fps2_To_Degrees_Gain) 
return  Fin_Deflections_Degrees  is 

begin 

return  Fin_Def lections  Degrees 

(Left  *  AcceTeration_Commands_Fps2  (Right)); 
end 

function  (Left  :  Acceleration_Commands_Mps2; 

Right  :  Mps2_To_Degrees_Gain) 
return  Fin_Deflections_Degrees  is 

begin 

return  Fin_Deflections_Degrees 

(Left  *  Acceleration_Commands_Mps2  (Right)); 

end  M*'1 ; 

function  (Left  :  Acceleration__Commands_Gees; 

Right  :  Gees_To_Degrees_Gain) 
return  Fin_Deflections_Degrees  is 

begin 

return  Fin  Deflections  Degrees 

^Lef t  *  AcceIeration_Commands_Gees  (Right)); 

end 

function  (Left  :  Acceleration_Commands_Fps2; 

Right  :  Fps2_To_Radians_Gain) 
return  Fin_Deflecticns_Radians  is 

begin 

return  Fin_Deflections  Radians 

(Left  *  AcceIeration_Commands  Fps2  (Right)); 

end  ; 

function  (Left  :  Acceleration_Coraraands_Mps2; 

Right  :  Mps2_To_Radians_Gain) 
return  Fin_Deflections_Radians  is 

begin 

return  Fin_Deflections  Radians 

(Left  *  AcceIeration_Coramands_Mps2  (Right)); 

end  ; 

function  (Left  :  Acceleration_Commands_Gees; 

Right  :  Gees_To_Radians_Gain) 
return  Fin_Deflections_Radians  is 

begin 

return  Fin_Deflections  Radians 

(Left  *  AcceIeration_Commands_Gees  (Right)); 

end  ; 
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—  —  Feedback  Rates  *  Gain  =  =  >  Fin  Deflections- 


—  — Ambiguity  in  the  meaning  of  the  "*"  operator  has  required  changing  the 

—  — standard  order  for  explicit  type  conversion.  The  bodies  of  the  next  two 

—  — functions  perform  explicit  type  conversion  on  the  "Left"  operand  to  avoid 

—  — ambiguity  with  subprograms  derived  from  the  type  declarations  of  Feedback 

—  — Rates  Degrees  and  Feedback  Rates  Radians. 

function  (Left  s  Feedback_Rates_Degrees; 

Right  :  Dps_To  Degrees_Gain) 
return  Fin_DefIections_Degrees  is 

begin 

return  Fin_Deflections_Degrees 

(Dps_To_Degrees_Gain  (Left)  *  Right); 

end 

function  (Left  :  Feedback_Rates_Radians; 

Right  :  Rps_To  Radians_Gain) 
return  Fin_DefTections_Radians  is 

begin 

return  Fin_Deflections_Radians 

(Rps_To_Radians_Gain  (Left)  *  Right); 

end  ”*n; 


- Roll  Commands  *  Gain  =  =  >  Angle- 


function  (Left  ;  Roll_Commands_Degrees; 

Right  :  Gain_In_Degreis) 
return  Fin_Deflections_Degrees  is 

begin 

return  Fin_Deflections_Degrees 

(Left  *  Roll_Commands_Degrees  (Right)); 

end 

function  (Left  :  Roll_Commands_Radians; 

Right  :  Gain_Xn_Radians) 
return  Fin_Deflections_Radians  is 

begin 

return  Fin  Deflections_Radians 

TLeft  *  Roll_Commands_Radians  (Right)); 

end  ; 


—  — Acceleration  Commands  +  Acceleration  Feedbacks  =  =  >  Acceleration  Commands- 


function  "+H  (Left  :  Acceleration_Commands_Fps2; 

Right  :  Acceleration_Feedbacks_Fps2) 
return  Acceleration_Commands_Fps2  is 

begin 

return  Left  +  Acceleration_Commands_Fps2  (Right); 
end  "+"; 

function  "+"  (Left  :  Acceleration  Commands  Mps2; 
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Right  :  Acceleration_Feedbacks_Mps2) 
return  Acceleration_Commands_Mps2  is 

begin 

return  Left  +  Acceleration_Commands_Mps2  (Right); 

end 


function  "+"  (Left  :  Acceleration_Commands_Gees; 

Right  :  Acceleration_Feedbacks_Gees) 
return  Acceleration_Commands_Gees  is 

begin 

return  Left  +  Acceleration_Commands_Gees  (Right); 
end  "+"; 


—  — Acceleration  Commands  -  Acceleration  Feedbacks  =  =  >  Acceleration  Commands- 


function  u-n  (Left  :  Acceleration_Commands_Fps2; 

Right  :  Acceleration_Feedbacks_Fps2) 
return  Acceleration_Commands_Fps2  is 

begin 

return  Left  -  Acceleration  Commands  Fps2  (Right); 

end 


function  (Left  :  Acceleration_Commands_Mps2; 

Right  ;  Acceleration_Feedbacks_Mps2) 
return  Acceleration_Commands_Mps2  is 

begin 

return  Left  -  Acceleration_Commands  Mps2  (Right); 
end  ; 


function  (Left  :  Acceleration_Conunands_Gees; 

Right  :  Acceleration_Feedbacks_Gees) 
return  Acceleration_Commands_Gees  is 

begin 

return  Left  -  Acceleration  Commands  Gees  (Ri^ht); 
end  ; 


—  — Roll  Commands  -  Missile  Attitudes  —  =  >  Roll  Commands- 


function  (Left  :  Roll  Commands_Degrees ; 

Right  ;  MissIle_Attitudes_Degrees) 
return  Roll_Comraands_Degrees  is 

begin 

return  Left  -  Roll_Commands_Degrees  (Right); 

end 


function  (Left  :  Roll_Commands_Radians; 

Right  :  Missile_Attitudes_Radians) 
return  Roll_Commands_Radians  is 

begin 

return  Left  -  Roll_Commands  Radians  (Right); 

end 


end  Autopilot  Data  Types; 


WQfiWDU 
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3.3.2  NAVIGATION 
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3. 3. 2.1  COMMON_NAVIGATION_PARTS  (PACKAGE  BODY)  TLCSC  P001  (CATALOG  #P208-0) 

This  part,  which  is  designed  as  an  Ada  package,  contains  specifications  for  all 
CAMP  parts  which  can  be  used  in  either  a  Wander  Azimuth  or  a  North  Pointing 
navigation  coordinate  system  environment. 

The  decomposition  for  this  part  is  the  same  as  that  shown  in  the  Top-Level 
Design  Document. 


3. 3. 2. 1.1  REQUIREMENTS  ALLOCATION 

The  following  chart  summarizes  the  allocation  of  CAMP  requirements  to  this 
part: 


|  Name 

|  Requirement  Number 

1 

|  Altitude  Integration 

|  R002 

1 

j  Compute  Ground  Velocity 

|  R003 

t 

j  Compute  Gravitational  Acceleration  Lat  In 

j  R005 

1 

j  Compute  Gravitational  Acceleration  Sin  Lat  In 

j  R006 

1 

j  Compute  Heading 

|  R009 

1 

j  Update  Velocity 

i  R010 

1 

|  Compute  Scalar  Velocity 

1  R039 

1 

j  Compute_Rotation_Increments 

|  R157 

1 

3.3.2. 1.2  LOCAL  ENTITIES  DESIGN 
None. 

3. 3. 2. 1.3  INPUT/OUTPUT 
None. 

3. 3. 2. 1.4  LOCAL  DATA 
None. 

3. 3. 2. 1.5  PROCESS  CONTROL 
Not  applicable. 


3. 3. 2. 1.6  PROCESSING 

The  following  describes  the  processing  performed  by  this  part: 
package  body  Common_Navigation_Parts  is 


package  body  Altitude_Integration  is  separate; 
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function  Compute_Ground_Velocity 

(East_Velocity  :  Velocities; 

North_Velocity  :  Velocities)  return  Velocities  is  separate; 

function  Compute_Gravitational_Acceleration_Lat_In 
(Current_Altitude  :  Distances; 

Current_Latitude  :  Earth_Positions) 
return  Accelerations  is  separate; 

function  Compute_Gravi tational_Acceleration_Sin_Lat_In 
(Current_Altitude  :  Distances; 

Sin_Current_Latitude  :  Sin_Cos_Ratio) 
return  Accelerations  is  separate; 

function  Compute_Heading  (East_Velocity  :  Velocities; 

North_Velocity  :  Velocities) 
return  Headings  is  separate; 

package  body  Update_Velocity  is  separate; 

function  Scalar_Velocity  (Velocity  In  :  Velocity_Vectors) 

return  Velocities  is  separate; 

function  Compute_Rotation_Increments 

(Platform_Rotation_Rate  :  Angular  Velocity_Vectors; 

Delta_T.ime  :  Intervals) 

return  AngleVectors  is  separate; 

end  Common_Navigation  Parts; 


l3 

a 


3 

jj 


1 

a 

i 


/'Tx 


3.3.2. 1.7  UTILIZATION  OF  OTHER  ELEMENTS 
None. 


3. 3. 2. 1.8  LIMITATIONS 
None. 


3. 3. 2. 1.9  LLCSC  DESIGN 

3. 3. 2. 1.9.1  ALTITUDE_INTEGRATION  PACKAGE  DESIGN  (CATALOG  #P209-0) 

This  package  updates  the  altitude  using  trapezoidal  integration  of  the 
earth-relative  vertical  velocity.  It  has  the  ability  to  reinitialize  the 
previous  values  of  the  vertical  velocity  and  the  altitude,  and  the  ability  to 
calculate  a  new  value  for  altitude. 

The  decomposition  for  this  part  is  the  same  as  that  shown  in  the  Top-Level 
Design  Document. 


Ami 
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3. 3. 2. 1.9. 1.1  REQUIREMENTS  ALLOCATION 
This  part  meets  CAMP  requirement  R002. 


3. 3. 2. 1.9. 1.2  LOCAL  ENTITIES  DESIGN 
None. 


3. 3. 2. 1.9. 1.3  INPUT/OUTPUT 
GENERIC  PARAMETERS: 

The  following  generic  parameters  were  previously  defined  for  this  part's 
package  specification: 

Data  types: 


The  following  table  describes  the  generic  formal  types  required  by  this  part: 


|  Name 

Type  |  Description  | 

Distances 

floating  |  Data  type  used  to  define  distance  measurements 

point  type  j 

Intervals 

floating  |  Data  type  used  to  define  time  measurements 

point  type  j 

Velocities 

floating  j  Data  type  used  to  define  velocity  measurements 

point  type  j 

Data  objects: 

The  following  table  describes  the  generic  formal  objects  required  by  this  part: 


|  Name 

1  Type 

Value 

Description  | 

I  Default  Delta 

|  Intervals 

N/A 

Default  time  over  which  ] 

j  Time 

integration  is  to  be  performed  j 

|  Initial  Vertical 

j  Velocities 

N/A 

Initial  vertical  velocity  to  be  j 

|  Velocity 

used  by  the  Integrate  function  j 

j  Initial  Altitude 

j  Distances 

N/A 

Initial  altitude  to  be  used  by  j 

Integrate  function  j 

Subprograms : 

The  following  table  describes  the  generic  formal  subroutines  required  by  this 
part: 
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j  Name 

1  Type  ! 

|  Description  | 

|  »*'i 

1 

|  function 

1 

|  Multiplication  operator  used  to  define  the  operation:  j 
|  Velocities  *  Intervals  =>  Distances  j 

3. 3. 2. 1.9. 1.4  LOCAL  DATA 


Data  objects: 

The  following  table  describes  the  data  objects  maintained  by  this  part: 


|  Name 

1  Type 

l  Value 

|  Description 

1 

]  Previous 

|  Velocities 

|  N/A 

|  Previous  value  of  the 

vertical  | 

j  Vertical  Velocity 

1 

j  velocity 

1 

j  Previous  Altitude  j 

|  Distances 

j  N/A 

j  Previously  calculated 

altitude  | 

3. 3. 2. 1.9. 1.5  PROCESS  CONTROL 
Not  applicable. 


3. 3. 2. 1.9. 1.6  PROCESSING 

The  following  describes  the  processing  performed  by  this  part: 

separate  (Common_Navigation_Parts) 
package  body  Altitude_Integration  is 


—  — local  declarations- 


Previous_Vertical_Velocity  :  Velocities  :=*  Initial_Vertical_Velocity; 
Previous_Altitude  :  Distances  :=  Initial_Altitude; 

end  Altitude_Integration; 


3. 3. 2. 1.9. 1.7  UTILIZATION  OF  OTHER  ELEMENTS 
None. 


3. 3. 2. 1.9. 1.8 


LIMITATIONS 
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3. 3. 2. 1.9. 1.9  LLCSC  DESIGN 


None. 


3.3.2.1.9.1.10  UNIT  DESIGN 

3.3.2.1.9.1.10.1  REINITIALIZE  UNIT  DESIGN 

This  procedure  reinitializes  the  previous  values  for  vertical  velocity  and 
altitude.  These  values  are  required  by  the  Integrate  function. 


3.3.2.1.9.1.10.1.1  REQUIREMENTS  ALLOCATION 
This  part  meets  CAMP  requirement  R002. 


3.3.2.1.9.1.10.1.2  LOCAL  ENTITIES  DESIGN 


None. 


3.3.2.1.9.1.10.1.3  INPUT/OUTPUT 


FORMAL  PARAMETERS: 

The  following  table  describes  this  part's  formal  parameters: 


|  Name 

Type  | 

Value 

|  Description  | 

|  New  Vertical_ 
j  Velocity 

Velocity  | 

1 

In 

|  Value  to  be  used  to  reinitialize  | 

j  previous  vertical  velocity  j 

j  New  Altitude 

1  ~ 

Distance  | 

1 

In 

j  Value  to  be  used  to  reinitialize  j 

j  previous  altitude  j 

3.3.2.1.9.1.10.1.4  LOCAL  DATA 


None. 


3.3.2.1.9.1.10.1.5  PROCESS  CONTROL 


Not  applicable. 


3.3.2.1.9.1.10.1.6  PROCESSING 

The  following  describes  the  processing  performed  by  this  part: 

procedure  Reinitialize  (New_Vertical_Velocity  :  in  Velocities; 

New  Altitude  :  in  Distances)  is 


begin 
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Previous_Altitude  :=  New_Altitude; 

Previous_Vertical_Velocity  :=  New_Vertical_Velocity; 

end  Reinitialize; 


3.3.2.1.9.1.10.1.7  UTILIZATION  OF  OTHER  ELEMENTS 
UTILIZATION  OF  ANCESTRAL  ELEMENTS: 

The  following  tables  describe  the  elements  used  by  this  part  but  defined  in  one 
or  more  ancestral  units: 

Data  types: 

The  following  table  summarizes  the  types  required  by  this  part  and  defined  as 
generic  parameters  to  the  enclosing  package  body: 


|  Name  |  Type  |  Description 


Distances 

Intervals 

Velocities 


floating 
point  type 
floating 
point  type 
floating 
point  type 


Data  type  used  to  define  distance  measurements 
Data  type  used  to  define  time  measurements 
Data  type  used  to  define  velocity  measurements 


Data  objects: 

The  following  table  describes  the  data  objects  required  by  this  part  and 
located  in  the  package  body  of  Altitude_Integration: 


|  Name 

|  Type 

|  Value 

|  Description  | 

|  Previous 

|  Velocities 

|  N/A 

|  Previous  value  of  the  vertical  | 

I  Vertical  Velocity 

1 

j  velocity  j 

j  Previous_Altitude 

|  Distances 

j  N/A 

j  Previously  calculated  altitude  j 

3.3.2.1.9.1.10.1.8  LIMITATIONS 


None. 


3.3.2.1.9.1.10.2  INTEGRATE  UNIT  DESIGN 

This  procedure  calculates  the  new  altitude  using  trapezoidal  integration  of  the 
earth-relative  vertical  velocity.  It  also  updates  previous  velocity  and 
altitude  variables. 
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3.3.2.1.9.1.10.2.1  REQUIREMENTS  ALLOCATION 
This  part  meets  CAMP  requirement  R002. 

3.3.2.1.9.1.10.2.2  LOCAL  ENTITIES  DESIGN 
None. 

3.3.2.1.9.1.10.2.3  INPUT/OUTPUT 
FORMAL  PARAMETERS: 

The  following  table  describes  this  part's  formal  parameters: 


|  Name 

1  Type 

|  Value 

|  Description  | 

|  Vertical  Velocity 

1 

|  Velocity 

1  1 

|  N/A 

|  Current  earth-relative  vertical 
|  velocity  cf  the  missile 

|  Delta  Time 

1 

|  Interval 

1 

|  N/A 

|  Time  interval  over  which  the 
(  integration  is  to  take  place 

3.3.2.1.9.1.10.2.4 

LOCAL  DATA 

Data  objects: 

The  following  table  describes  the  data  objects  maintained  local  to  this  part: 


|  Name 

1  Type 

Value 

|  Description  | 

|  Altitude 

|  Distances 

N/A 

|  Altitude  being  calculated  | 

3.3.2.1.9.1.10.2.5  PROCESS  CONTROL 
Not  applicable. 

3.3.2.1.9.1.10.2.6  PROCESSING 

The  following  describes  the  processing  performed  by  this  part: 

function  Integrate  (Vertical_Velocity  :  Velocities; 

Delta_Time  :  Intervals  :=  Default_Delta_Time) 

return  Distances  is 

— local  declarations 

Altitude  :  Distances; 
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ii.*. 


—  — begin  function  Integrate 
begin 

Altitude  :=  Previous_Altitude  + 

(  (Vertical_Velocity  +  Previous_Vertical_Velocity)  * 

(0.5  *  Delta_Time)  ); 

Previous_Vertical_Velocity  :=  Vertical_Velocity; 

Previous_Altitude  :  =  Altitude; 

return  Altitude; 

end  Integrate; 

3.3.2.1.9.1.10.2.7  UTILIZATION  OF  OTHER  ELEMENTS 
UTILIZATION  OF  ANCESTRAL  ELEMENTS: 

The  following  tables  describe  the  elements  used  by  this  part  but  defined  in  one 
or  more  ancestral  units: 

Data  types: 

The  following  table  summarizes  the  types  required  by  this  part  and  defined  as 
generic  parameters  to  the  enclosing  package  body: 


Description 

Data  type  used  to  define  distance  measurements 
Data  type  used  to  define  time  measurements 
Data  type  used  to  define  velocity  measurements 


Data  objects: 

The  following  table  describes  the  data  objects  required  by  this  part  and 
located  in  the  package  body  of  Altitude  Integration: 


|  Name 

Type  | 

|  Distances 

floating  ! 

point  type  j 

j  Intervals 

floating  j 

point  type  j 

j  Velocities 

floating  | 

point  type  | 

|  Name 

1  Type 

|  Value 

|  Description  | 

|  Previous 

|  Velocities 

|  N/A 

|  Previous  value  of  the  vertical  | 

j  Vertical  Velocity 

1  velocity  j 

j  Previous_Altitude 

|  Distances 

j  N/A 

j  Previously  calculated  altitude  j 
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3.3.2.1.9.1.10.2.8  LIMITATIONS 
None. 


3. 3. 2. 1.9. 2  UPDATE_VELOCITY  PACKAGE  DESIGN  (CATALOG  #P214-0) 

This  package  contains  the  subroutines  required  to  compute  the  current  velocity 
(in  vector  form). 

It  consists  of  reinitialize,  a  procedure  which  reinitializes  the  value  of  the 
Velocity  vector,  and  Update,  a  function  which  computes  the  current  velocity 
vector  given  the  velocity  change  vector,  the  time  interval  over  which  the 
velocity  changes  were  accumulated,  Coriolis  acceleration  vector,  and 
acceleration  due  to  gravity. 

The  computations  performed  by  the  update  function  are  as  follows: 

New  Velocity  :=  Previous  Velocity  +  Delta  Velocity  - 
(  (Coriolis  Acceleration  - 

Gravitational  Acceleration)  *  Delta  Time) 

The  decomposition  for  this  part  is  the  same  as  that  shown  in  the  Top-Level 
Design  Document. 


3. 3. 2. 1.9. 2.1  REQUIREMENTS  ALLOCATION 
This  part  meets  CAMP  requirenient  R010. 


3. 3. 2. 1.9. 2. 2  LOCAL  ENTITIES  DESIGN 
None. 


3.3.2. l^. 3  INPUT/OUTPUT 
GENERIC  PARAMETERS: 

This  part's  generic  parameters  were  previously  defined  in  the  package 
specification  of  Common_Navigation_Parts: 

Data  types: 

The  following  table  describes  the  generic  formal  types  required  by  this  part: 


i 

f) 


h.AlWLTrtfriti!  nr  frv.tr  iT-ff  Hi  •Vi’AfirVfc'.  **Vtn*ia X&U.  litfAUiWA 
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|  Name 

1  Type 

1 

Description  | 

|  Velocity  Vectors 

|  private 

1 

Contains  the  east,  north,  and  vertical | 

$ 

'a. 

components  of  the  missile's  current  j 

*1 

1 

velocity  j 

| 

j  Intervals 

j  floating 

1 

Data  type  used  to  define  time  j 

i  point  type 

1 

measurements  j 

j  Accelerations 

j  floating 

1 

Data  type  used  to  define  j 

I  point  type 

1 

acceleration  measurements  1 

j  Indices 

j  scalar  type 

Used  to  dimension  Acceleration  Vectors j 

J 

j  Acceleration_Vectors 

j  array 

1 

Array  of  Accelerations  “  j 

jj 

Data  objects:  & 

The  following  table  describes  the  generic  formal  objects  required  by  this  part:  ) 


|  Name 

1  Type 

|  Value 

|  Description  | 

|  Default 

|  Acceleration 

|  N/A 

|  Vector  defining  acceleration  due  to 

j  Gravity 

|  Vectors 

1 

j  gravity  (positive  in  the  up 

1 

j  direction) 

|  Initial 

j  Velocity 

j  N/A 

j  Initial  velocity  values  used  by 

j  Velocity 

j  Vectors 

1 

j  Update  function 

Subprograms: 

The  following  table  describes  the  generic  formal  subroutines  (operators) 
required  by  this  part: 


|  Name 

Type 

Description  |  } 

..  . .  ; 

|  »*» 

function 

Vector-scalar  multiplication  operator  defining  |  \ 

the  operation:  j 

Acceleration_Vectors  *  Intervals  =>  j  \ 

Velocity  Vectors  j  ■ 

I  Sparse  Right 

function 

Subtraction  operator  operating  on  acceleration  j  jj 

i  XY  Subtract 

vectors  where  the  assumption  is  made  that  the  j  jj 

third  element  of  the  right  input  vector  equals  j  $ 

0  |  \ 

Vector-vector  addition  operator  defining  the  j  | 

operation:  j  j 

Velocity_Vectors  +  Velocity_Vectors  =>  j  : 

Velocity  Vectors  j  \ 

1  ,,  +  M 

function 

1 

function 

Vector-vector  subtraction  operator  defining  the  |  ; 

operation:  j  j 

Velocity_Vectors  -  Velocity_Vectors  =>  j  ; 

Velocity_Vectors  j  \ 

m 


SE,.. 
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3. 3. 2. 1.9. 2. 4  LOCAL  DATA 
Data  objects: 

The  following  table  describes  the  data  objects  maintained  by  this  part: 


|  Name  | 

Type 

|  Value 

|  Description  | 

|  Previous  Velocity  | 

1  "  1 

1  1 

Velocity  Vector 

|  N/A 

|  Velocity  vector  calculated  | 
j  during  previous  update  j 
|  operation  j 

3. 3. 2. 1.9. 2. 5  PROCESS  CONTROL 
Not  applicable. 

3. 3. 2. 1.9. 2. 6  PROCESSING 

The  following  describes  the  processing  performed  by  this  part: 

separate  (Common_Navigation_Parts) 
package  body  Update_Velocity  is 

- local  variables- 

Previous_Velocity  :  Velocity_Vectors  :=>  Initial_Velocity; 
end  Update_Velocity; 

3. 3. 2. 1.9. 2. 7  UTILIZATION  OF  OTHER  ELEMENTS 
None. 

3. 3. 2. 1.9. 2. 8  LIMITATIONS 
None. 

3. 3. 2. 1.9. 2. 9  LLCSC  DESIGN 
None. 


3.3.2.1.9.2.10  UNIT  DESIGN 
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3.3.2.1.9.2.10.1  REINITIALIZE  UNIT  DESIGN 

This  procedure  reinitializes  the  previous  velocity  vector  required  by  the 
update  function. 

3.3.2.1.9.2.10.1.1  REQUIREMENTS  ALLOCATION 
This  part  meets  CAMP  requirement  R010. 

3.3.2.1.9.2.10.1.2  LOCAL  ENTITIES  DESIGN 
None. 

3.3.2.1.9.2.10.1.3  INPUT/OUTPUT 
FORMAL  PARAMETERS: 

The  following  table  describes  this  part's  formal  parameters: 

|  Name  |  Type  |  Value  |  Description  | 

|  Nev_Velocity  |  Velocity_Vectors|  In  |  New  value  for  the  previous_  ! 

|  ~  |  jj  velocity_vector  j 

3.3.2.1.9.2.10.1.4  LOCAL  DATA 
None. 

3.3.2.1.9.2.10.1.5  PROCESS  CONTROL 
Not  applicable. 

3.3.2.1.9.2.10.1.6  PROCESSING 

The  following  describes  the  processing  performed  by  this  part: 
procedure  Reinitialize  (New_Velocity  :  in  Velocity_Vectors)  is 
begin 

Previous_Velocity  :=  New_Velocity; 


end  Reinitialize; 
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3.3.2.1.9.2.10.1.7  UTILIZATION  OF  OTHER  ELEMENTS 
UTILIZATION  OF  ANCESTRAL  ELEMENTS: 

The  following  tables  describe  the  elements  used  by  this  part  but  defined  in  one 
or  more  ancestral  units: 

Data  types: 

The  following  table  summarizes  the  types  required  by  this  part  and  defined  as 
generic  parameters  to  the  enclosing  package  body: 


|  Name 

1  Type 

|  Description  | 

|  Velocity  Vectors 
! 

1 

|  private 

1 

1 

|  Contains  the  east,  north,  and  vertical | 
j  components  of  the  missile's  current  j 
|  velocity  j 

Data  objects: 

The  following  table  summarizes  the  objects  required  by  this  part  and  defined  in 
the  enclosing  package  body: 

|  Name 

1  Type 

|  Value  |  Description  | 

|  Previous_Velocity  |  Velocity_Vectors |  N/A  |  Velocity  vector  calculated 
1  ill  during  previous  update 

!  1  I  I  operation 


3.3.2.1.9.2.10.1.8  LIMITATIONS 
None. 


3.3.2.1.9.2.10.2  UPDATE  UNIT  DESIGN 

This  function  computes  the  current  velocity  vector  given  the  velocity  change 
vector,  the  time  interval  over  which  the  velocity  changes  were  accumulated,  the 
coriolis  acceleration  vector,  and  the  acceleration  due  to  gravity. 

The  computations  performed  are  as  follows: 

New  Velocity  :=  Previous  Velocity  +  Delta  Velocity  - 
(  (Coriolis  Acceleration  - 

Gravitational  Acceleration)  *  Delta  Time) 


3.3.2.1.9.2.10.2.1  REQUIREMENTS  ALLOCATION 
This  part  meets  CAMP  requirement  R010. 
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3.3.2.1.9.2.10.2.2  LOCAL  ENTITIES  DESIGN 


None. 


3.3.2.1.9.2.10.2.3  INPUT/OUTPUT 
FORMAL  PARAMETERS: 

The  following  table  describes  this  part's  formal  parameters: 


Name 

1  Type  | 

Value 

|  Description 

Nominal  Delta_ 
Velocity 

|  Velocity_Vectors| 

!  1 

1  i 

N/A 

|  Contains  the  east,  north,  and 
j  vertical  components  of  the 
j  change  in  the  missile's 
j  velocity 

Coriolis 

Acceleration 

j  Acceleration_  [ 

j  Vectors  1 

1  ! 

N/A 

j  Contains  the  east,  north,  and 
j  vertical  components  of  the 
j  coriolis  acceleration 

Delta  Time 

j  Intervals  J 

1  1 

N/A 

j  Time  interval  over  which  the 
|  velocity  changes  occurred 

|  3. 3. 2. 1.9. 2. 1G. 2. 4  LOCAL  DATA 

I 

|  None. 

i 

i 

3.3.2.1.9.2.10.2.5  PROCESS  CONTROL 

Not  applicable. 

l 

3.3.2.1.9.2.10.2.6  PROCESSING 

j 

The  following  describes  the  processing  performed  by  this  part: 

i 

j  function  Update  (Nominal_Delta  Velocity  :  Velocity_Vectors; 

i  Coriolis_AcceTeration  :  Acceleration_Vectors; 

j  Delta_Time  :  Intervals; 

Gravity  :  Acceleration_Vectors 

:=  Default_Gravity) 

return  Velocity_Vectors  is 


— declaration  of  variables- 


Current_Velocity  :  Velocity_Vectors; 
Temp_A_V  :  Acceleration_Vectors; 

Temp_V_V  :  Velocity_Vectors; 


- beginning  of  function  Update 
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begin 

Temp_A_V  :=  Sparse_Right_XY_Subtract(Coriolis ^Acceleration,  Gravity); 
Temp_V_V  : =  Temp_A_V  *  Delta_Time; 

Current_Velocity  :=  Previous_Velocity  + 

Nominal_Delta_Velocity  - 
Temp_V_V ; 

Previous_Velocity  :=  Current_Velocity; 
return  Current_Velocity; 
end  Update; 


3.3.2.1.9.2.10.2.7  UTILIZATION  OF  OTHER  ELEMENTS 
UTILIZATION  OF  ANCESTRAL  ELEMENTS: 

The  following  tables  describe  the  elements  used  by  this  part  but  defined  in  one 
or  more  ancestral  units: 

Data  types: 

The  following  table  summarizes  the  types  required  by  this  part  and  defined  as 
generic  parameters  to  the  enclosing  package  body: 


|  Name 


|  Type  |  Description 


Veloci ty_Vectors 

Intervals 

Accelerations 

Indices 

Acceleration  Vectors 


private 


floating 
point  type 
floating 
point  type 
scalar  type 
array 


Contains  the  east,  north,  and  vertical 
components  of  the  missile's  current 
velocity 

Data  type  used  to  define  time 
measurements 

Data  type  used  to  define 
acceleration  measurements 
Used  to  dimension  Acceleration_Vector 
Array  of  Acceleration 


Data  objects: 

The  following  table  summarizes  the  objects  required  by  this  part  and  defined  as 
generic  parameters  to  the  enclosing  package: 


|  Name  |  Type  |  Value  |  Description 


|  Default_  |  Acceleration_  |  N/A  |  Vector  defining  acceleration  due  to  j 
!  Gravity  |  Vectors  |  |  gravity  (positive  in  the  up  j 

j  j  direction)  j 


CAMP  Software  Detailed  Design  Document 


P; 


The  following  table  summarizes  the  objects  required  by  this  part  and  defined  in 
the  enclosing  package  body: 


|  Name 

1  Type  | 

Value 

|  Description  | 

|  Previous  Velocity 

1  ~  1 
1  ! 

|  Velocity  Vectors | 

1  1 

1  1 

N/A 

|  Velocity  vector  calculated  | 

|  during  previous  update  j 

j  operation  j 

Subprograms  and  task  entries: 

The  following  table  summarizes  the  subroutines  and  task  entries  required  by 
this  part  and  defined  as  generic  parameters  to  the  enclosing  package: 


|  Name 

Type 

Description 

«*" 

function 

Vector-scalar  multiplication  operator  defining 
the  operation: 

Acceleration_Veetors  *  Intervals  => 

Velocity  Vectors 

Sparse_Right 
XY_Sub tract 

function 

Subtraction  operator  operating  on  acceleration 
vectors  where  the  assumption  is  made  that  the 
third  element  of  the  right  input  vector  equals 

0 

Vector-vector  addition  operator  defining  the 
operation: 

Velocity_Vectors  +  Velocity_Vectors  =*> 

Veloci  ty*"Vectors 

tt  +  ?t 

function 

1f_l* 

function 

Vector-vector  subtraction  operator  defining  the 
operation: 

Veloci ty_Vec tors  -  Velocity  Vectors  => 

Veloci ty_Vec tors 

3.3.2.1.9.2.10.2.8  LIMITATIONS 
None. 

3.3.2.1.9.2.10.3  CUSRENT_VELOCITY  UNIT  DESIGN 

This  function  returns  the  previous  velocity  (i.e.  the  current  velocity). 
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3.3.2.1.9.2.10.3.2  LOCAL  ENTITIES  DESIGN 
None. 

3.3.2.1.9.2.10.3.3  INPUT/OUTPUT 
None. 

3.3.2.1.9.2.10.3.4  LOCAL  DATA 
None. 

3.3.2.1.9.2.10.3.5  PROCESS  CONTROL 
Not  applicable. 

3.3.2.1.9.2.10.3.6  PROCESSING 

The  following  describes  the  processing  performed  by  this  part: 
function  Current_Velocity  return  Velocity_Vectors  is 
begin 

return  Previous_Veloeity; 
end  Current_Velocity; 

3.3.2.1.9.2.10.3.7  UTILIZATION  OF  OTHER  ELEMENTS 

UTILIZATION  OF  OTHER  ELEMENTS  IN  TOP-LEVEL  COMPONENT: 

The  following  tables  describe  the  elements  used  by  this  part  but  defined 
elsewhere  in  this  top-level  component: 

Data  types: 

The  following  table  summarizes  the  types  required  by  this  part  and  defined  as 
generic  parameters  to  the  enclosing  package  body: 


|  Name 

1  Type 

|  Description  | 

|  Velocity  Vectors 

1 

1 

|  private 

1 

1 

|  Contains  the  east,  north,  and  vertical | 
j  components  of  the  missile's  current  j 
|  velocity  j 

Data  objects: 
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The  following  table  summarizes  the  objects  required  by  this  part  and  defined  in 
the  enclosing  package  body: 


j  Name  |  Type  |  Value  |  Description 


|  Previous_Velocity  |  Velocity_Vectors |  N/A  |  Velocity  vector  calculated  | 
!  |  ||  during  previous  update  j 
!  i  II  operation  j 


3.3.2.1.9.2.10.3.8  LIMITATIONS 


None. 


3.3.2.1.10  UNIT  DESIGN 

3.3.2.1.10.1  COMPUTE_GROUND_VELOCITY  UNIT  DESIGN  (CATALOG  #P210-0) 

This  function  calculates  the  ground  (i.e.  horizontal)  velocity  of  the  missile 
from  the  east  and  north  components  of  the  missile  velocity. 

The  ground  velocity  is  as  computed  as  follows: 

GVel  :»  Sqrt(EVel**2  +  NVel  **2) 

where  EVel  *  East  Velocity 
NVel  »  North  Velocity 


3.3.2.1.10.1.1  REQUIREMENTS  ALLOCATION 
This  part  meets  CAMP  requirement  R003. 


3.3.2.1.10.1.2  LOCAL  ENTITIES  DESIGN 


None. 


3.3.2.1.10.1.3  INPUT/OUTPUT 
GENERIC  PARAMETERS: 

The  following  generic  parameters  were  previously  defined  in  the  package 
specification  of  Common_Navigation_Parts: 

Data  types: 

The  following  table  describes  the  generic  formal  types  required  by  this  part: 
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|  Name 

Type  | 

Description  | 

|  Velocities 

|  Velocity  Squared 

1 

floating  J 
point  type  j 
floating  j 
point  type  j 

Data  type  used  to  define  velocity  | 

measurements  | 

Data  type  resulting  from  multiply  two  j 

objects  of  type  velocity  j 

Subprograms : 

The  following  table 
part: 

describes  the  generic  formal  subroutines  required  by  this 

|  Name  |  Type 

|  Description 

1 

|  |  function 

1  1 

|  Sqrt  |  function 

|  Multiplication  operator  defining  the  operation:  | 

|  Velocities  *  Velocities  =>  Velocity_Squared  j 

j  Square  root  function  "  j 

FORMAL  PARAMETERS: 

The  following  table 

describes  this 

part's  formal  parameters: 

|  Name  | 

Type  |  Mode  j  Description  | 

|  East_Velocity  |  Velocities  |  In 
j  North_Velocity  j  Velocities  j  In 

|  Current  east  velocity  of  missile  | 

|  Current  north  velocity  of  missile  j 

3.3.2.1.10.1.4  LOCAL  DATA 


None. 


3.3.2.1.10.1.5  PROCESS  CONTROL 
Not  applicable. 


3.3.2.1.10.1.6  PROCESSING 

The  following  describes  the  processing  performed  by  this  part: 

separate  (Comrnon_Navigation_Parts) 
function  Compute_Ground_Velocity 

(East_Velocity  :  Velocities; 

North_Velocity  :  Velocities)  return  Velocities  is 


begin 

return  Sqrt(East_Velocity  *  East  Velocity  + 
NortK_Velocity  *  NortK_Velocity); 
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end  Compute_Ground_Velocity; 

3.3.2.1.10.1.7  UTILIZATION  OF  OTHER  ELEMENTS 
None. 

3.3.2.1.10.1.8  LIMITATIONS 
None. 

3.3.2.1.10.2  COMPUTE_GRAVITATIONAL  ACCELERATION  LAT  IN  UNIT  DESIGN  (CATALOG 
#P211-0)  “  ~ 

The  part  computes  the  vertical  acceleration  due  to  gravity  given  the  missile' 
current  altitude  and  latitude.  The  calculations  performed  are  as  follows: 

Grav  :=  NGrav  *  (1  -  (2*Alt/EER)  +  (GCF  *  Sin(Lat)**2)  ) 

where  NGrav  =  Nominal  Gravity 

GCF  =  Gravity  Coefficient 
EER  *  Earth  Equatorial  Radius 

3.3.2.1.10.2.1  REQUIREMENTS  ALLOCATION 
This  part  meets  CAMP  requirement  R005. 

3.3.2.1.10.2.2  LOCAL  ENTITIES  DESIGN 
None. 

f 

3.3.2.1.10.2.3  INPUT/OUTPUT 
GENERIC  PARAMETERS: 

This  part's  generic  parameters  were  previously  defined  in  the  package 
specification  for  Common_Navigation_Parts. 

Data  types: 

The  following  table  describes  the  generic  formal  types  required  by  this  part: 
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|  Name 

1  Type 

Description 

1 

|  Accelerations 

|  floating 

Data  type  used  to  define 

acceleration 

1 

j  point  type 

measurements 

|  Distances 

j  floating 

Data  type  used  to  define 

distance 

j  point  type 

measurements 

j  Earth  Positions 

j  floating 

Data  type  used  to  define 

latitude  and 

1 

j  point  type 

longitude  measurements 

1 

j  Inverse 

j  floating 

Data  type  used  to  define 

1/distance 

1 

j  Distances 

j  point  type 

measurements 

1 

j  Real 

j  floating 

Data  type  of  Gravity  Coefficient 

1 

|  point  type 

|  Sin  Cos  Ratio 

j  floating 

Data  type  used  to  define 

the  results  of  a 

j  point  type 

Sin  calculation 

1 

Data  objects: 

The  following  table  describes  the  generic  formal  objects  required  by  this  part: 


|  Name 

1 

Type  | 

Value 

|  Description  | 

3 

| 

4 

|  Nominal_Gravity 

Accelerations | 

N/A 

|  Value  of  absolute  mean  | 

j  normal  gravity  j 

3 

j  0ne_0\ir_Earth_ 

Distances  j 

N/A 

j  Value  for  1/earth  equatorial! 

j  Radius 

j  radius  j 

j  Gravity_Coefficient 

Real  | 

N/A 

j  Coefficient  of .gravity  j 

V 

1 

Subprograms: 

The  following  table  describes  the  generic  formal  subroutines  required  by  this 
part: 


|  Name  |  Type  |  Description 


function 

function 

function 

function 


Multiplication  operator  defining  the  operation: 
Accelerations  *  Real  =>  Accelerations 
Multiplication  operator  defining  the  operation: 
Sin_C'os_Ratio  *  Real  =>  Real 
Division  operator  defining  the  operation: 
Distances  *  Inverse_Distances  =>  Real 
Sine  function 


FORMAL  PARAMETERS: 

The  following  table  describes  this  part's  formal  parameters: 
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|  Name 

Type  | 

Mode 

|  Description 

1 

|  Current 

1 

_Altitude 

Distances  j 

1 

In 

|  Current  altitude 
j  missile 

of 

the  | 

I 

|  Current 

1 

_Latitude 

Earth  Positions! 

I 

In 

j  Current  latitude 
j  missile 

of 

the  | 

1 

3.3.2.1.10.2.4  LOCAL  DATA 
Data  objects: 

The  following  table  describes  the  data  objects  maintained  by  this  part: 


|  Name 

Type 

Value 

|  Description  | 

|  Sin_of  Lat 

Sin_Cos_Ratio 

N/A 

|  Sine  of  CurrentJLatitude  | 

3.3.2.1.10.2.5  PROCESS  CONTROL 
Not  applicable. 


3.3.2.1.10.2.6  PROCESSING 

The  following  describes  the  processing  performed  by  this  part: 

separate  (Common_Navigation_Parts) 
function  Compute~Gravi tational_Acceleration_Lat_In 
(Current_Altitude  :  Distances; 

Current_Latitude  :  Earth  Positions)  return  Accelerations  is 


- declaration  of  variables- 


Sin_of_Lat  :  Sin_Cos_Ratio; 

l 

i  _ 

— begin  function  Compute_Gravitational_Acceleration_Lat_In 

i 

•  begin 

'  Sin_of_Lat  :«  Sin(Current_Latitude) ; 

!  return  Nominal  Gravity  * 

■  (1.0  - 

|  (Current_Altitude+Current_Altitude)  *  One  Over_Earth_Radius  + 

i  Sin  of  Lat  *  Sin  of  Lat  *  Gravity  Coefficient 

I  >5 

i 


end  Compute  Gravitational  Acceleration  Lat  In; 
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3.3.2.1.10.2.7  UTILIZATION  OF  OTHER  ELEMENTS 
None. 

3.3.2.1.10.2.8  LIMITATIONS 
None. 

3.3.2.1.10.3  COMPUTE_GRAVITATIONAL  ACCELERATION  SIN  LAT  IN  UNIT  DESIGN  (CATALOG 
#P212-0)  ~  “ 

Given  the  missile's  current  altitude  and  the  sine  of  the  current  latitude,  this 
function  computes  the  vertical  acceleration  due  to  gravity.  The  computations 
are  performed  as  follows: 

Grav  :  =  NGrav  *  (1  -  (2  *  Alt/EER)  +  (GCF  *  SinLat**2))) 

where  NGrav  =  Nominal  Gravity 

GCF  =  Gravity  Coefficient 
EER  =  Earth  Equatorial  Radius 

3.3.2.1.10.3.1  REQUIREMENTS  ALLOCATION 
This  part  meets  CAMP  requirement  R006. 

3.3.2.1.10.3.2  LOCAL  ENTITIES  DESIGN 
None. 

3.3.2.1.10.3.3  INPUT/OUTPUT 
GENERIC  PARAMETERS: 

This  part's  generic  parameters  were  previously  defined  in  the  package 
specification  of  Common_Navigation_Parts: 

Data  types: 

The  following  table  describes  the  generic  formal  types  required  by  this  part: 
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1 

Name 

1  Type 

Description 

1 

1 

Accelerations 

|  floating 

Data  type  used  to  define 

acceleration 

1 

1 

j  point  type 

measurements 

1 

1 

Distances 

j  floating 

Data  type  used  to  define 

distance 

1 

1 

j  point  type 

measurements 

1 

1 

Inverse 

j  floating 

Data  type  used  to  define 

1/distance 

i 

1 

Distances 

j  point  type 

measurements 

1 

Real 

j  floating 

Data  type  of  Gravity  Coefficient 

1 

|  point  type 

1 

1 

Sin  Cos  Ratio 

j  floating 

Data  type  used  to  define 

the  results  of  a 

1 

1 

j  point  type 

Sin  calculation 

1 

Data  objects: 

The  following  table  describes  the  generic  formal  objects  required  by  this  part: 


|  Name 

1 

Type 

|  Value 

|  Description  [ 

|  Nominal  Gravity 

1 

Accelerations!  N/A 

|  Value  of  absolute  mean  | 

1 

|  normal  gravity  [ 

|  One  Over  Earth 

Distances 

|  N/A 

j  Value  for  1/earth  equatorial! 

j  Radius 

1 

i 

j  radius  j 

j  Gravity_Coefficient 

1 

Real 

|  N/A 

j  Coefficient  of  gravity  j 

Subprograms : 

The  following  table  describes  the  generic  formal  subroutines  required  by  this 
part: 


|  Name  |  Type  |  Description 


n*n 

ll*M 


function 

function 

function 


Multiplication  operator  defining  the  operation: 
Accelerations  *  Real  =■>  Accelerations 
Multiplication  operator  defining  the  operation: 
Sin_Cos_Ratio  *  Real  =*>  Real 
Division-operator  defining  the  operation: 
Distances  *  Inverse  Distances  =>  Real 


FORMAL  PARAMETERS: 

The  following  table  describes  this  part's  formal  parameters: 


|  Name 

1  TyPe 

|  Mode  | 

Description  I 

|  Current  Altitude 

1 

|  Distances 

!  In 

1 

Current  altitude  of  the 
missile 

j  Sin  Current  Latitude 

I  Sin  Cos  Ratio 

1  In 

1 

j  Sine  of  the  current 

1  " 

1  “ 

latitude  of  the  missile 
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3.3.2.1.10.3.4  LOCAL  DATA 
None. 


3.3.2.1.10.3.5  PROCESS  CONTROL 
Not  applicable. 


3.3.2.1.10.3.6  PROCESSING 

The  following  describes  the  processing  performed  by  this  part: 
separate  ( Common_Nav i ga t i on_Par t s ) 

function  Compute_Gravitational_Acceleration_Sin_Lat_In 
(Current_Altitude  ~  :  Distances; 

Sin_Current_Latitude  :  Sin_Cos_Ratio) 
return  Accelerations  is 

begin 

return  Nominal_Gravity  * 

(1.0  - 

(Current_Altitude+Current_Altitude)  *  One  Over_Earth_Radius  + 
Sin_Currsnt_Latitude  *  Sin_Current_Latitu3e  * 
Gravity_Coefficient 

)  * 

end  Compute_Gravitational_Acceleration_Sin_Lat_In; 


3.3.2,1.10.3.7  UTILIZATION  OF  OTHER  ELEMENTS 
None. 


3.3.2.1.10.3.8  LIMITATIONS 
None. 


3.3.2.1.10.4  COMPUTE_HEADING  UNIT  DESIGN  (CATALOG  #P213-0) 

This  function  computes  the  missile's  current  heading  given  the  missile's 
current  east  and  north  velocities. 

The  computations  are  performed  as  follows: 

Hdg  :=  Arctan(EVel  /  NVel) 

where  EVel  =  East  velocity 
NVel  =  North  velocity 
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3.3.2.1.10.4.1  REQUIREMENTS  ALLOCATION 
This  part  meets  CAMP  requirement  R009. 


3.3.2.1.10.4.2  LOCAL  ENTITIES  DESIGN 


None. 


3.3.2.1.10.4.3  INPUT/OUTPUT 
GENERIC  PARAMETERS: 

This  part's  generic  parameters  were  previously  defined  in  the  package 
specification  of  Common_Navigation_Parts. 

Data  types: 

The  following  table  describes  the  generic  formal  types  required  by  this  part: 


|  Name 

Type 

Description  | 

|  Velocities 

floating 

Data  type  defining  velocity  measurements 

! 

point  type 

|  Headings 

floating 

Data  type  defining  angular  measurement 

1 

point  type 

Subprograms : 

The  following  table  describes  the  generic  formal  subroutines  required  by  this 
part: 


|  Name 

Type 

Description  | 

|  Arctan 

function 

Arctangent  function  | 

FORMAL  PARAMETERS: 

The  following  table  describes  this  part's  formal  parameters : 


|  Name 

1  Type  | 

|  Mode 

|  Description  | 

|  East  Velocity 

|  Velocities 

1  In 

|  Current  east  velocity  of  missile  | 

j  NortK_Velocity 

|  Velocities 

1  In 

[  Current  north  velocity  of  missile  ] 
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3.3.2.1.10.4.4  LOCAL  DATA 


None. 


3.3.2.1.10.4.5  PROCESS  CONTROL 
Not  applicable. 


3.3.2.1.10.4.6  PROCESSING 

The  following  describes  the  processing  performed  by  this  part: 

separate  (Common_Navigation_Parts) 

function  Compute_Heading  (East  Velocity  :  Velocities; 

NortK_Velocity  :  Velocities)  return  Headings  is 


begin 

- operation  performed  will  be  y/x  (want  East/North) 

return  Arctan2  (Y  =>  East  Velocity, 

X  =■>  NortK_Velocity); 

end  Compute_Heading; 


3.3.2.1.10.4.7  UTILIZATION  OF  OTHER  ELEMENTS 
None. 


3.3.2.1.10.4.8  LIMITATIONS 


None. 


3.3.2.1.10.5  SCALAR_VELOCITY  UNIT  DESIGN  (CATALOG  #P215-0) 

This  function  computes  the  scalar  velocity  given  a  velocity  vector. 


3. 3. 2. 1.10. 5. .1  REQUIREMENTS  ALLOCATION 
This  part  meets  CAMP  requirement  R039. 


3.3.2.1.10.5.2  LOCAL  ENTITIES  DESIGN 


None. 


3.3.2.1.10.5.3  INPUT/OUTPUT 


GENERIC  PARAMETERS: 
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The  following  generic  parameters  were  previously  defined  when  this  part  was 
specified. 

Data  types: 

The  following  table  describes  the  generic  formal  types  required  by  this  part: 


|  Name  | 

Type 

|  Description  | 

|  Velocities  ! 

1  1 
|  Velocity  Vectors | 

1  "  1 

floating 
point  type 
private 

|  Data  type  defining  velocity  measurements  | 

!  1 

|  Data  type  containing  a  velocity  vector  j 

j  values  i 

Subprograms : 

The  following  table  describes  the  generic  formal  subroutines  required  by  this 
part: 


|  Name  | 

Type 

Description  | 

|  Vector  | 

function 

Function  used  to  calculate  the  length  of  a  vector  | 

1 

|  Length  j 

FORMAL  PARAMETERS: 

The  following  table  describes  this  part's  formal  parameters: 

|  Name  |  Type  |  Mode  |  Description  | 

|  Velocity_In  |  Velocity_Vectors j  In  j  Missile's  current  velocity  vector  | 

3.3.2.1.10.5.4  LOCAL  DATA 
None. 

3.3.2.1.10.5.5  PROCESS  CONTROL 
Not  applicable. 

3.3.2.1.10.5.6  PROCESSING 

The  following  describes  the  processing  performed  by  this  part: 
separate  (Common_Navigation_Parts) 

function  Scalar_Velocity  (Velocity_In  :  Velocity_Vectors) 

return  Velocities  is 


begin 
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k. 


return  Vector_Length(Velocity_In) ; 
end  Scalar  Velocity; 


3.3.2.1.10.5.7  UTILIZATION  OF  OTHER  ELEMENTS 


None. 


3.3.2.1.10.5.8  LIMITATIONS 


None. 


3.3.2.1.10.6  C0MPUTE_R0TATI0N  INCREMENTS  (FUNCTION  BODY)  UNIT  DESIGN  (CATALOG 
#P216-0) 

This  function  computes  the  rotation  rate  increments  of  the  navigation 
coordinate  system  with  respect  to  inertial  space. 


3.3.2.1.10.6.1  REQUIREMENTS  ALLOCATION 
This  part  meets  CAMP  requirement  R157. 


3.3.2.1.10.6.2  LOCAL  ENTITIES  DESIGN 
None. 


3.3.2.1.10.6.3  INPUT/OUTPUT 
GENERIC  PARAMETERS: 

The  following  generic  parameters  were  previously  defined  when  this  part  was 
specified. 

Data  types: 

The  following  table  describes  the  generic  formal  types  required  by  this  part: 


|  Name  |  Type  |  Description 


Angle_Vectors  j 

private 

type 

1 

Angular_Velocity_Vectors | 

private 

type 

Intervals  I 

floating 

1 

point  type 

Vector  containing  the  east,  north, 
vertical  components  of  the 
rotation  rate  increments 
Vector  containing  the  east,  north, 
and  vertical  components  of  the 
total  platform  rate 
Data  type  defining  time 
measurements 
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Subprograms : 

The  following  table  describes  the  generic  formal  subroutines  (operators) 
required  by  this  part: 


|  Name  |  Left  Input  Type 


|  Right  Input  Type  |  Result  Type 


j  Angular  Velocity  Vectors]  Intervals 


]  Angle_Vectors 


FORMAL  PARAMETERS: 

The  following  table  describes  the  formal  parameters  of  this  part 


|  Mode  |  Description 


Platform_ 

Rotation_Rate 

Delta  Time 


Angular_ 

Velocity_Vectors 

Intervals 


Contains  the  nominal 
components  of  the  total 
rotation  rate 

Time  interval  over  which  the 
increments  are  to  be 
calculated 


3.3.2.1.10.6.4  LOCAL  DATA 


None. 


3.3.2.1.10.6.5  PROCESS  CONTROL 


Not  applicable. 


3.3.2.1.10.6.6  PROCESSING 

The  following  describes  the  processing  performed  by  this  part: 

separate  ( Common_Naviga t ion_Par t s ) 
function  Compute_Rotation_Increments 

(Pla"tform_Rotation_Rate  :  Angular  Velocity_Vectors 
DeltaJTime  ~  :  Intervals) 

return  Angle_Vectors  is 

begin 

return  Platform  Rotation  Rate  *  Delta  Time; 


end  Compute  Rotation  Increments; 
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3.3.2.1.10.6.7  UTILIZATION  OF  OTHER  ELEMENTS 
None. 

3.3.2.1.10.6.8  LIMITATIONS 
None. 
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package  body  Common_Navigation_Parts  is 

package  body  Altitude_Integration  is  separate; 

function  Compute_Ground_Velocity 

(East_Velocity  :  Velocities; 

North_Velocity  :  Velocities)  return  Velocities  is  separate; 

function  Compute_Gravitational_Acceleration_Lat_In 
(Current_Altitude  :  Distances; 

Current__Latitude  :  Earth_Positions) 
return  Accelerations  is  separate; 

function  Compute_Gravitational_Acceleration_Sin_Lat_In 
(Current_Altitude  :  Distances; 

Sin_Current_Latitude  :  Sin_Cos_Ratio) 
return  Accelerations  is  separate; 

function  Compute_Heading  (East_Velocity  :  Velocities; 

North_Velocity  :  Velocities) 
return  Headings  is  separate; 

package  body  Update_Velocity  is  separate; 

function  Scalar_Velocity  (Velocity  In  :  Velocity_Vectors) 

return  Velocities  is  separate; 

function  Compute_Rotation_Increments 

(PlatformJRotationRate  J  Angular  Velocity  Vectors; 

Delta_Time  :  Intervals) 

return  Angle  Vectors  is  separate; 

end  Common_Navigation_Parts; 
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separate  (Common_Navigation_Parts) 
package  body  Altitude_Integration  is 


—  —  local  declarations- 


Previous_Vertical_Velocity  :  Velocities  :=  Initial_Vertical_Velocity; 
Previous_Altitude  :  Distances  :=  Initial_Altitude; 

pragma  PAGE; 

procedure  Reinitialize  (Nev_Vertical_Velocity  :  in  Velocities; 

New  Altitude  :  in  Distances)  is 


begin 

Previous_Altitude  :=  New_Altitude; 

Previous_Vertical_Velocity  :=  New_Vertical_Velocity; 

end  Reinitialize; 

pragma  PAGE; 

function  Integrate  (Vertical_Velocity  :  Velocities; 

Delta_Time  s  Intervals  :=  Default_Delta_Time) 

return  Distances  is 


’  —  local  declarations 


Altitude  s  Distances; 


—  — begin  function  Integrate 


begin 

Altitude  :=  Previous_Altitude  + 

(  (Vertical  Velocity  +  Previous_Vertical_Velocity)  * 
(0.5  *  DeIta_Tirae)  ); 

Previous_Vertical_Velocity  :=  Vertical_Velocity; 
Previous_Altitude  :=  Altitude; 

return  Altitude; 

end  Integrate; 

end  Altitude_Integration; 


3 


,ii 

«< 
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separate  (Common_Navigation_Parts) 
function  Compute_Ground_Velocity 

(East_Velocity  :  Velocities; 

North_Velocity  :  Velocities)  return  Velocities 


begin 

return  Sqrt(East_Velocity  *  East  Velocity  + 
North_Velocity  *  NortE_Velocity); 

end  Compute_Ground_Velocity; 
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separate  ( Common_Nav iga  t i on_Par  t  s ) 
function  Compute_Gravitational_Acceleration_Lat_In 
(Current_Altitude  :  Distances; 

Current  Latitude  :  Earth  Positions)  return  Accelerations  is 


—  — declaration  of  variables- 


Sin  Of  Lat  ;  Sin  Cos  Ratio; 


— begin  function  Compute  Gravitational  Acceleration  Lat  in 


begin 

Sin_Of_Lat  :=  Sin(Current_Latitude); 
return  Nominal  Gravity  * 

(1.0  - 

(Current_Altitude+Current_Altitude)  *  One  Over_Earth_Radius  + 
Sin  Of  Lat  *  Sin  Of  Lat  *  Gravity  Coefficient 
)i 

end  Compute_Gravi tational_Acceleration_Lat_In ; 
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separate  (Common_Naviga  t ion_Par  ts ) 

function  Compute_Gravi tational_Acceleration_Sin_Lat_In 
(Current_Altitude  :  Distances; 

Sin_Current_Latitude  :  Sin_Cos_Ratio) 
return  Accelerations  is 

begin 

return  Nominal_Gravity  * 

(1.0  - 

(Current_Altitude+Current_Altitude)  *  One  Over_Earth_Radius 
Sin_Current_Latitude  *  Sin_Current_Latitu<Ie  * 

Gravity  Coefficient 
)  5 

end  Compute  Gravitational  Acceleration  Sin  Lat  In; 
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separate  ( Common_Naviga  t ion_Par  ts ) 

function  Compute_Heading  (East_Velocity  :  Velocities; 

North_Velocity  :  Velocities)  return  Headings  is 

begin 

—  — operation  performed  will  bey  lx  (want  East/North) 
return  Arctan2  (Y  =>  East_Velocity, 

X  =>  North  Velocity); 


end  Compute_Heading; 
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separate  ( Common_Navi ga t i on_Par t s ) 
package  body  Update_Velocity  is 

—  — local  variables- 

Previous_Velocity  :  Velocity_Vectors  :=  Initial_Velocity; 
pragma  PAGE; 

procedure  Reinitialize  (Nev_Velocity  :  in  Velocity_Vectors)  is 
begin 

Previous_Velocity  :=  Nev_Velocity; 
end  Reinitialize; 


■i 


5 

; 


pragma  PAGE; 

function  Update  (Nominal_Delta  Velocity  :  Velocity  Vectors; 

Coriolis_AcceTeration  :  Acceleration_Vectors; 
Delta_Time  :  Intervals; 

Gravity  :  Acceleration_Vectors 

:»  Default_Gravity) 

return  Veloci ty_Vectors  is 


— declaration  of  variables- 


Current_Velocity  :  Veloci ty_Vec tors; 
Temp_A_V  :  Acceleration_Vectors; 

Temp_V_V  :  Veloci ty_Vectors; 


—  — beginning  of  function  Update 


begin 

Temp_A_V  :=  Sparse_Right_Xy_Subtract(Coriolis_Acceleration,  Gravity); 
Temp_V_V  :=  Temp_A_V  *  Delta_Time; 

Current_Velocity  :=  Previous_Velocity  + 

Norainal_Delta_Velocity  - 
Temp_V_V ; 

Previous_Velocity  :=  Current_Velocity; 
return  Current_Velocity; 
end  Update; 
pragma  PAGE; 

function  Current_Velocity  return  Veloci ty_Vectors  is 
begin 


l 


£ 

fi 


i 


A  J*  i ai  A  &  A 
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return  Previous_Velocity; 
end  Current_Velocity; 


end  Update_Velocity; 


CAMP  Software  Detailed  Design  Document 


separate  ( Common_Naviga  t ion_Par  ts ) 

function  Scalar_Velocity  (Velocity_In  :  Velocity_Vectors) 

return  Velocities  is 


begin 


return  Vector_Length(Veloci ty_In) ; 


end  Scalar  Velocity; 
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separate  ( Common_Naviga  t ion_Par  ts ) 
function  Compute_Rotation_Increments 

(Platform_Rotation_Rate  :  Angular_Velocity_Vectors 
Delta_Time  :  Intervals) 

return  Angle_Vectors  is 

begin 

return  Platform_Rotation_Rate  *  Delta_Time; 
end  Compute_Rotation_Increments; 
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3. 3. 2. 2  NORTH  POINTING  NAVIGATION  PARTS  (BODY)  TLCSC  P003  (CATALOG  #P257-0) 

This  part,  which  is  a  package  body,  contains  all  the  CAMP  parts  which  can  be 
used  in  a  north  pointing  navigation  coordinate  system  environment. 

The  decomposition  for  this  part  is  the  same  as  that  shown  in  the  Top-Level 
Design  Document. 

3. 3. 2. 2.1  REQUIREMENTS  ALLOCATION 

The  following  chart  describes  the  allocation  of  requirements  to  the  LLCSC's  in 
this  TLCSC. 


|  Name  |  Requirements  Allocation  | 


|  Compute  Coriolis  Acceleration 

|  R008 

1 

j  Radius  of  Curvature 

|  R036 

1 

|  Total  Platform  Rotation  Rates 

1  R012 

1 

|  Earth  Rotation  Rate 

1  R014 

I 

j  Earth  Relative  Rotation  Rates 

!  R026 

j  Latitude  Integration 

i  R027 

1 

j  Longitude_Integration 

|  R031 

1 

3. 3. 2. 2. 2  LOCAL  ENTITIES  DESIGN 
None. 

3. 3. 2. 2. 3  INPUT/OUTPUT 
None. 

4 

3. 3. 2. 2. 4  LOCAL  DATA 
None. 

3. 3. 2. 2. 5  PROCESS  CONTROL 
Not  applicable. 

3. 3. 2. 2. 6  PROCESSING 

The  following  describes  the  processing  performed  by  this  part: 
package  body  North_Pointing_Navigation_Parts  is 
end  Nor th_Pointing_Navigation_Par ts } 
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3. 3. 2. 2. 7  UTILIZATION  OF  OTHER  ELEMENTS 
None. 

3. 3. 2. 2. 8  LIMITATIONS 
None. 

3. 3. 2. 2. 9  LLCSC  DESIGN 

3. 3. 2. 2. 9.1  RADIUS_OF_CURVATURE  PACKAGE  DESIGN  (CATALOG  #P259-0) 

This  LLCSC  contains  a  function  which  computes  the  radius  of  curvature  vector 
given  the  current  altitude  and  current  latitude. 

The  decomposition  for  this  part  is  the  same  as  that  shown  in  the  Top-Level 
Design  Document. 


3. 3. 2. 2. 9. 1.1  REQUIREMENTS  ALLOCATION 
This  part  meets  CAMP  requirement  R036i 


3. 3. 2. 2. 9. 1.2  LOCAL  ENTITIES  DESIGN 
None. 


3. 3. 2. 2. 9. 1.3  INPUT/OUTPUT 
GENERIC  PARAMETERS: 

The  following  generic  parameters  were  previously  defined  in  the  package 
specification  of  North_Pointing_Navigation. 

Data  types: 

The  following  table  summarizes  the  generic  formal  types  required  by  this  part: 


MSI 

§3 
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Name 

Earth_Positions 

Distances 

Inverse_ 

Distances 

Indices 

Distance_Vectors 

Real 

Sin  Cos  Ratio 


Description  | 

Data  type  of  latitude  and  longitude  values  | 

Data  type  of  distance  measurements  j 

Data  type  of  1/distance  measurements  | 

Used  to  index  arrays  | 

Array  of  distances  j 

Used  to  define  Earth_Flattening_Coefficient j 

Data  type  of  results  from  a  sin/cos  j 

function  i 


floating 
point  type 
floating 
point  type 
floating 
point  type 
scalar  type 
array 
floating 
point  type 
floating 
point  type 


Data  objects: 

The  following  table  summarizes  the  generic  formal  objects  required  by  this 
part: 


|  Description 


Earth_Radius 
Earth  Flattening_ 
Coefficient 
One  Over_Earth_ 
Radius 


Radius  of  the  earth 
Coefficient  of  earth  flattening 

Value  of  1/radius  of  the  earth 


Subprograms : 

The  following  table  summarizes  the  generic  formal  subroutines  required  by  this 
part: 


Name 

1  Type 

n*n 

|  function 

m*ii 

|  function 

ii*ii 

|  function 

It /ft 

1 

|  function 

Sin_Cos 

1 

|  procedure 

|  Description 


Defines  the  operation: 

Real  *  Sin_Cos_Ratio  :=  Real 
Defines  the-operation: 

Distances  *  Inverse_Distances  :=  Real 
Defines  the  operation: 

Distances  *  Real  :=  Distances 
Defines  the  operation: 

Distances  /  Real  :*■  Distances 

Procedure  returning  the  sine  and  cosine  of  an  input 
earth  position 


FORMAL  PARAMETERS: 

The  following  table  describes  the  formal  parameters  to  the  unit  contained  in 
this  part: 
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i  Name 

Type  1 

Mode 

|  Description 

1 

|  Latitude 

Earth  Positions! 

In 

|  Current  latitude 

of 

the 

missile  | 

j  Altitude 

Distances  j 

In 

j  Current  altitude 

of 

the 

missile  j 

3. 3. 2. 2. 9. 1.4  LOCAL  DATA 
Data  objects: 

The  following  table  describes  the  data  objects  maintained  by  this  part: 

|  Name  |  Type  |  Description  | 

|  E  |  Indices  |  Index  to  first  element  in  vector  | 

j  N  j  Indices  j  Index  to  second  element  in  vector  j 

j  V  j  Indices  j  Index  to  last  element  in  vector  j 
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— unit  body 


function  Compute  (Latitude  :  Earth_Positions; 

Altitude  :  Distances) 
return  Distance  Vectors  is 


— declaration  of  variables- 


Cos_of_Lat  :  Sin  Cos_Ratio;  | 
Flat_x_Sin_Squared  :  Real;  I 
Long_Denominator  s  Real;  | 
Radius_of_Curvature  :  Distance_Vectors;  | 
Short  Denominator  :  Real;  1 
Sin  ol  Lat  :  Sin  Cos_Ratio;  i 


— begin  function  Compute 


begin 

Sin_Cos(Latitude,  Sin_of_Lat,  Cos_of_Lat); 

Flat_x_Sin_Squared  :=  Ear th_Flat tening_Coef f icient  * 

(Sin_of_Lat  *  Sin_of_Lat); 

Short_Denominator  :*  1.0  - 

Altitude  *  0ne_0ver_Earth_Radius  - 
Flat_x_Sin_Squared ; 

Long_Denominatcr  :=  Short_Denominator  + 

2.0  *  Earth_Flattening_Coefficient  * 

(Cos_of_Lat*Cos_of_Lat) ; 

Radius_of_Curvature(E)  :*  Earth_Radius  /  Short_Denominator; 

Radius_of_Curvature(N)  :*  Earth_Radius  /  Long_Denominator;  , 

Radius_of_Curvature(V)  :=  Earth_Radius  *  (1.0  -  Flat_x_Sin_Squared)  +  j 

Altitude;  ~  j 

return  Radius  of  Curvature; 

“  “  l 

end  Compute;  l 

end  Radius  of_Curvature;  ] 


3. 3. 2. 2. 9. 1.7  UTILIZATION  OF  OTHER  ELEMENTS  1 

None.  I 

i 

4 

1 

'i 


safe  ■  •“ 


‘XK1 
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3. 3. 2. 2. 9. 1.8  LIMITATIONS 
None. 
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3. 3. 2. 2. 9. 1.9  LLCSC  DESIGN 


None. 


3.3.2.2.9.1.10  UNIT  DESIGN 


None. 


3. 3. 2. 2. 9. 2  EARTH_R0TATI0NRATE  PACKAGE  DESIGN  (CATALOG  IP261-0) 

This  LLCSC  contains  a  function  which  computes  the  rotation  rate  of  the  Earth 
given  the  current  latitude. 

The  calculations  performed  are  as  follows: 

Oraega(E)  :=  0.0 

Omega(N)  :*  Earth_Angular_Velocity  *  cos(Lat) 

Omega(U)  :  =  Earth_Angular__Velocity  *  sin(Lat) 

The  decomposition  for  this  part  is  the  same  as  that  shown  in  the  Top-Level 
Design  Document. 


3. 3. 2. 2. 9. 2.1  REQUIREMENTS  ALLOCATION 
This  part  meets  CAMP  requirement  R014. 


3. 3. 2. 2. 9. 2. 2  LOCAL  ENTITIES  DESIGN 
None. 


3. 3. 2. 2. 9. 2. 3  INPUT/OUTPUT  J 

1 

GENERIC  PARAMETERS:  j 

| 

The  following  generic  parameters  were  previously  defined  in  the  package  | 

specification  for  North_Fointing_Navigation_Parts:  | 

4 

\ 

Data  types:  3 

I 

The  following  table  describes  the  generic  formal  types  required  by  this  part:  i 
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|  Name  | 

Type 

1 

Description 

1 

|  Angular  j 

floating 

1 

Data  type  defining  angular  velocity 

j  Velocities  i 

point  type 

1 

measurements 

j  Indices  j 

discrete 

1 

Index  into  arrays 

|  Angular  j 

j  Velocity  Vectors j 

array 

1 

Array  of  angular  velocities 

j  Earth  Positions  j 

floating 

1 

Data  type  defining  longitude/latitude 

i  1 

point  type 

1 

measurements 

i  Sin  Cos  Ratio  1 

floating 

1 

Data  type  defining  results  of  sin/cos 

point  type 

1 

functions 

Data  objects: 

The  following  table  describes  the  generic  formal  objects  required  by  this  part: 


|  Name 

1  Type 

|  Value 

I  Description  | 

|  Earth  Rate 

1 

|  Angular 
j  Velocities 

|  N/A 

1 

|  Rate  of  rotation  of  the  earth  j 

1  i 

Subprograms: 

The  following  table  describes  the  generic  formal  subroutines  required  by  this 
part: 


j  Name  |  Type  |  Description 


»*» 


Sin  Cos 


function  |  Multiplication  operator  defining  the  operation: 
j  Angular_Velocities  *  Sin_Cos_Ratio  := 
j  Angular_Velocities 

procedure  j  Returns  the  sine  and  cosine  of  an  input  earth 
|  position 


FORMAL  PARAMETERS : 

The  following  table  describes  the  formal  parameters  to  the  unit  in  this 
package: 


|  flame 

Type  | 

Mode 

|  Description  | 

|  Latitude 

Earth_Positions| 

In 

|  Current  latitude  of  the  missile  | 

3. 3. 2. 2. 9. 2. 4  LOCAL  DATA 

Data  objects: 


n 

i 

v 

Vi 

I 

•j 


3 

l 

3 


* 

1 


V 
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The  following  table  describes  the  data  objects  maintained  by  this  part: 


|  Name 

1  Type 

|  Description 

1 

1  E,  N,  U 

|  Indices 

|  Indices  into  1st,  2nd,  and 

last  | 

1 

1 

j  elements  of  vector 

1 

The  following  table  describes  the  data  objects  maintained  local 

to  the  unit  in 

this  package; 

|  Name 

1  Type 

|  Description 

1 

1  Cos  Of  Lat 

1  Sin  Cos  Ratio 

|  Cosine  of  input  latitude. 

1 

1  Rotation  Rate 

1  Angular  Velocity 

j  Value  being  calculated  and 

returned  j 

! 

!  Vectors 

I 

I 

j  Sin  Of  Lat 

j  Sin  Cos  Ratio 

j  Sine  of  input  latitude 

1 

3. 3. 2. 2. 9. 2. 5  PROCESS  CONTROL 
Not  applicable. 

3. 3. 2. 2. 9. 2. 6  PROCESSING 

The  following  describes  the  processing  performed  by  this  part; 
package  body  Earth_Rotation_Rate  is 

— declaration  of  variables- 

E  :  constant  Indices  :=  Indices' FIRST; 

N  ;  constant  Indices  :=  Indices' SUCC(E); 

V  :  constant  Indices  :=  Indices'LAST; 


— unit  body 


function  Compute  (Latitude  ;  Earth_Positions) 

return  Angular  Velocity  Vectors  is 


— declaration  of  variables- 


Cos_of_Lat  :  Sin_Cos_Ratio; 
Rotation_Rate  ;  Angular_Velocity_Vectors; 
Sin  of  Lat  :  Sin  Cos  Ratio; 
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— beginning  of  function- 
begin 

Sin_Cos (Latitude,  Sin_of_Lat,  Cos_of_Lat); 

Rotation_Rate(E)  :=  0.0; 

Rotation_Rate(N)  :=  Earth_Rate  *  Cos_of_Lat; 
Rotation_Rate(V)  :=  Earth_Rate  *  Sin_of_Lat; 

return  Rotation_Rate; 

end  Compute; 

end  Earth  Rotation  Rate; 


3. 3. 2. 2. 9. 2. 7  UTILIZATION  OF  OTHER  ELEMENTS 
None. 

3. 3. 2. 2. 9. 2. 8  LIMITATIONS 


None. 

3. 3. 2. 2. 9. 2. 9  LLCSC  DESIGN 
None. 


3.3.2.2.9.2.10  UNIT  DESIGN 


None. 


3. 3. 2. 2. 9. 3  EARTH_RELATIVE  NAVIGATION  ROTATION  RATE  PACKAGE  DESIGN  (CATALOG 
#P262-0)  ~ 

This  package  contains  a  function  which  computes  the  rotation  rate  of  the 
navigation  coordinate  system  with  respect  to  the  Earth. 

The  calculations  performed  are  as  follows; 

Rho(E)  :=  -  Vel(N)  /  Radius_of_Curvature(N) 

Rho(N)  ;=  Vel(E)  /  Radius_of~Curvature(E) 

Rho(V)  :=  Rho(N)  *  tan(lat) 

where  Vel  =  the  missile's  current  velocity 
Lat  =  latitude  of  the  missile 


The  decomposition  for  this  part  is  the  same  as  that  shown  in  the  Top-Level 
Design  Document. 


i 


i 
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3. 3. 2. 2. 9. 3.1  REQUIREMENTS  ALLOCATION 
This  part  meets  CAMP  requirement  R026. 

3. 3. 2. 2. 9. 3. 2  LOCAL  ENTITIES  DESIGN 
None. 

3. 3. 2. 2. 9. 3. 3  INPUT/OUTPUT 
GENERIC  PARAMETERS: 

The  following  generic  parameters  were  previously  defined  in  the  package 
specification  of  North_Pointing_Navigation_Parts: 

Data  types: 

The  following  table  describes  the  generic  formal  types  required  by  this  part: 


|  Name 


Type  |  Description 


Indices 

Angular_ 
Velocities 
Angular_Veloci ty 
Vectors 
Distances 

Distance_Vectors 

Earth_Positions 

Tan_Ratio 

Velocities 

Velocity_Vectors 


floating 
point  type 
floating 
point  type 
array 

floating 
point  type 
array 
floating 
point  type 
floating 
point  type 
floating 
point  type 
array 


Used  to  dimension  imported  vector  types 
Data  type  of  angular  velocity  objects 
Array  of  angular  velocities 
Data  type  of  distance  measurements 
Array  of  distances 

Data  type  of  longitude/latitude  objects 
Data  type  of  results  of  tangent  function 
Data  type  of  velocity  measurements 
Array  of  velocities 


Subprograms : 

The  following  table  describes  the  generic  formal  subroutines  required  by  this 
part: 


j  Name  |  Type  |  Description 


ii*n 

n /\\ 

Tan 


function  |  Multiplication  operator  defining  the  operation: 

|  Angular_Velocities  *  Tan_Ratio  :=  Angular_Velocities 
function  |  Division  operator  defining  the  operation: 

j  Velocities  /  Distances  :=  Angular_Velocities 
function  j  Tangent  function 
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i 

FORMAL  PARAMETERS: 

The  following  table  describes  the  formal  parameters  to  the  unit  contained  in 
this  part: 


j  Name 


Type  |  Mode  |  Description 


I 


Missile_Velocity 
Radius  of  Curvature 


Velocity  Vectors)  In 

I 

Distance  Vectors |  In 


Latitude 


Earth  Positions 


In 


Missile's  current  velocity 
vector 

East  and  north  components 
of  the  radius  of 
curvature 

Current  latitude  of  the 
missile 


3. 3. 2. 2. 9. 3. 4  LOCAL  DATA 
Data  objects: 

The  following  table  describes  the  data  objects  maintained  by  this  part: 


|  Name  |  Type 

|  Description  | 

)  E,  N,  U  |  Indices 

1  1 

|  Indices  into  1st,  2nd,  and  last  positions  of  | 
j  a  vector  ♦  j 

The  following  table  describes 
contained  in  this  part: 

the  data  objects  maintained  local  to  the  unit 

|  Name  |  Type 

|  Description  | 

|  Rho  |  Angular_Veloci ty_ 

|  j  Vectors 

|  Vector  being  calculated  and  returned  | 

1  1 

3. 3. 2. 2. 9. 3. 5  PROCESS  CONTROL 
Not  applicable. 

3. 3. 2. 2. 9. 3. 6  PROCESSING 

The  following  describes  the  processing  performed  by  this  part: 
package  body  Earth_Relative_Navigation_Rotation_Rate  is 


— declaration  of  variables- 
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E  :  constant  Indices  :=  Indices 'FIRST; 

N  :  constant  Indices  :=  Indices ' SUCC(E) ; 

V  :  constant  Indices  :=  Indices'LAST; 


— unit  body 


function  Compute  (Missile_Velocity  :  Velocity_Vectors; 

Radius_of_Curvature  :  Distance_Vectors; 
Latitude  :  Earth_Positions) 
return  Angular_Velocity_Vectors  is 


— declaration  of  variables- 


Rho  :  Angular_Velocity_Vectors; 


— beginning  of  function- 


begin 

Rho(e)  :=«  -  MissileVelocity(n)  /  Radius  of_Curvature(n); 
Rho(n)  :=  MissileVelocity(e)  /  Radius_oI_Curvature(e) ; 
Rho(v)  :=  Rho(n)  *  Tan(Latitude); 

return  Rho; 

end  Compute; 

end  Earth_Relative_Navigation_Rotation_Rate; 


3. 3. 2. 2. 9. 3. 7  UTILIZATION  OF  OTHER  ELEMENTS 
None. 

3. 3. 2. 2. 9. 3. 8  LIMITATIONS 


None. 

3. 3. 2. 2. 9. 3. 9  LLCSC  DESIGN 


None. 


3.3.2.2.9.3.10  UNIT  DESIGN 
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3. 3. 2. 2. 9. 4  LATITUDE  INTEGRATION  PACKAGE  DESIGN  (CATALOG  #P263-0) 


This  LLCSC,  which  is  a  package,  calculates  the  missile's  current  latitude  using 
trapezoidal  integration  of  the  east  component  of  the  Earth-relative  rotation 
rate  of  the  navigation  coordinate  system.  This  part  can  be  used  when  a  north 
pointing,  local  level  coordinate  system  is  being  used  for  navigation. 

The  decomposition  for  this  part  is  the  same  as  that  shown  in  the  Top-Level 
Design  Document. 


3. 3. 2. 2. 9. 4.1  REQUIREMENTS  ALLOCATION 
This  part  meets  CAMP  requirement  R027. 


3. 3. 2. 2. 9. 4. 2  LOCAL  ENTITIES  DESIGN 
None. 


3. 3. 2. 2. 9. 4. 3  INPUT/OUTPUT 
GENERIC  PARAMETERS: 

Previously  defined  in  package  specification  for  NorthJPointingNavigation- 

T> _ A _  ‘ 


Data  types: 

The  following  table  describes  the  generic  formal  types  required  by  this  part: 


|  Name 

Type 

Description  | 

|  Angular 

floating 

Data  type  of  angular  velocity  objects  j 

j  Velocities 

point  type 

|  Earth_Positions 

floating 
point  type 

Data  type  of  latitude  and  longitude  values j 

j  Intervals 

floating 
point  type 

Data  type  of  time  values  j 

Data  objects: 

The  following  table  describes  the  generic  formal  objects  required  by  this  part: 


Name 

Type 

Va: 

Default 

Intervals 

N/A 

Delta 

Initial 

Angular 

N/A 

East  RKo 

Velocities 

Initial 

Earth 

N/A 

Latitude 

Position 

Description 


Default  time  over  which  integration  is 
to  take  place 

Initial  value  of  the  East  component  of 
earth-relative  rotation 
Initial  value  of  the  latitude 
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Subprograms: 

Tne  following  table  describes  the  generic  formal  subroutines  required  by  this 
part : 


|  Name 

1  Type 

|  Description  | 

j  »*» 

1 

|  function 

1 

Multiplication  operator  defining  the  operation:  j 

Angular  Velocities  *  Intervals  :=  Earth  Positions  j 

3. 3. 2. 2. 9. 4. 4  LOCAL  DATA 


Data  objects: 

The  following  table  describes  the  data  objects  maintained  local  to  this  part: 


|  Name 

Type 

Value 

|  Description 

1 

|  Previous 

Angular 

N/A 

|  Previous  value  of  the 

East  component  | 

|  East  Rho 

Velocities 

]  of  the  Earth-relative 

rotation  j 

1  Previous 

Earth 

N/A 

j  Previous  value  of  the 

latitude  j 

j  Latitude 

Positions 

1 

1 

3. 3. 2. 2. 9. 4. 5  PROCESS  CONTROL 
Not  applicable. 


3-3. 2. 2. 9. 4. 6  PROCESSING 

The  following  describes  the  processing  performed  by  this  part: 
package  body  Latitude_Iritegration  is 


— local  declarations 


Previous_East_Rho  :  Angular _Veloci ties  :=  Initiai_East_Rho; 
Previous_Latitude  :  Earth_Positions  Initial_Latitude; 

end  Latitude_Integration; 


3. 3. 2. 2. 9. 4. 7  UTILIZATION  OF  OTHER  ELEMENTS 


None. 


V*  V*  ‘ *  *  V  •  V  V  V  “  J*  \*  V  V  *>  V  V  %**  V  *  •"!>  V S>V>^  fcV  , 
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3. 3. 2. 2. 9. 4. 8  LIMITATIONS 


None. 


3. 3. 2. 2. 9. 4. 9  LLCSC  DESIGN 


None. 


3.3.2.2.9.4.10  UNIT  DESIGN 

3.3.2.2.9.4.10.1  REINITIALIZE  UNIT  DESIGN 

This  unit,  which  is  a  procedure,  reinitializes  the  previous  values  of  the  east 
component  of  the  earth-relative  rotation  and  latitude. 


3.3.2.2.9.4.10.1.1  REQUIREMENTS  ALLOCATION 
This  unit  partially  meets  CAMP  requirement  R027. 


3.3.2.2.9.4.10.1.2  LOCAL  ENTITIES  DESIGN 


None. 


i 

% 


3.3.2.2.9.4.10.1.3  INPUT/OUTPUT 
FORMAL  PARAMETERS; 


The  following  table  describes 

this  part' 

s  formal  parameters; 

|  Name 

Type 

|  Mode  | 

Description  | 

|  East  Rho 

Angular 

1  1"  | 

Initial  value  for  | 

I 

1 

Velocities 

i  1 

previous  east  rho  j 

1 

|  Latitude 

Earth  Positions 

1  In  ! 

Initial  value  lor  f 

3 

1 

I  1 

previous_latitude  | 

I 


3.3.2.2.9.4.10.1.4  LOCAL  DATA 


None. 

3.3.2.2.9.4.10.1.5  PROCESS  CONTROL 
Not  applicable. 


% 

5 


r  *'  “  ft  Him  ti X* t A 
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3.3.2.2.9.4.10.1.6  PROCESSING 

The  following  describes  the  processing  performed  by  this  part: 

procedure  Reinitialize  (East_Rho  :  in  Angular_Velocities; 

Latitude  :  in  Earth_Positions)  is 

begin 

Pravious_East_Rho  :=  East_Rho; 

Previous_Latitude  :=  Latitude; 

end  Reinitialize; 


3.3.2.2.9.4.10.1.7  UTILIZATION  OF  OTHER  ELEMENTS 
UTILIZATION  OF  ANCESTRAL  ELEMENTS: 

The  following  tables  describe  the  elements  used  by  this  part  but  defined  in  one 
or  more  ancestral  units: 

Data  types: 

The  following  table  summarizes  the  types  required  by  this  part  and  defined  as 
generic  formal  types  in  the  package  specification  for  Latitude_Integration: 


(.  Name 

1 

Type 

Description  | 

|  Angular 

1 

floating 

Data  type  of  angular  velocity  objects  j 

|  Velocities 

1 

point  type 

j  Earth_Positions 

1 

1 

floating 
point  type 

Data  type  of  latitude  and  longitude  values | 

j  Intervals 

1 

1 

floating 
point  type 

Data  type  of  time  values  i 

Data  objects: 

The  following  table  describes  the  data  objects  required  by  this  part  and 
maintained  local  to  the  Latitude  Integration  package. 


|  Name  |  Type  |  Value  |  Description 


|  Previous_  |  Angular_  j  N/A  |  Previous  value  of  the  East  component 

|  East_Rho  j  Velocities  j  j  of  the  Earth-relative  rotation 

j  Previous_  j  Earth_  j  N/A  j  Previous  value  of  the  latitude 

j  Latitude  j  Positions  j  j 
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£ 

} 

3.3.2.2.9.4.10.1.8  LIMITATIONS  1 

j 

None.  | 

3.3.2.2.9.4.10.2  INTEGRATE  UNIT  DESIGN  j 

This  unit,  which  is  a  function,  calculates  a  new  latitude  through  trapezoidal 
integration.  J 


3.3.2.2.9.4.10.2.1 


;i 

*1 


REQUIREMENTS  ALLOCATION 


This  unit  partially  meets  CAMP  requirement  R027.  j 

3.3.2.2.9.4.10.2.2  LOCAL  ENTITIES  DESIGN  j 


None. 

3.3.2.2.9.4.10.2.3  INPUT/OUTPUT 


FORMAL  PARAMETERS: 


'A 

The  following  table  describes  this  part's  formal  parameters:  j 

I 


|  Name 

1 

Type 

|  Mode 

|  Description 

1 

> 

i 

|  East  Rho 

1 

Angular 

1  In 

|  Current  value  of  the  east 

■ 

5 

Velocities 

1 

j  component  of  the  earth-relative 

1 

J 

1 

|  rotation  rate  of  the  navigation 

1 

J  coordinate  system 

1 

\ 

|  Delta  Time 

Intervals 

1  In 

j  Time  interval  over  which  the 

1 

1 

1 

j  integration  is  to  take  place 

1 

1 

t 


3.3.2.2.9.4.10.2.4  LOCAL  DATA  : 

None.  f 

fl 

'j 

3.3.2.2.9.4.10.2.5  PROCESS  CONTROL  | 

Not  applicable. 


3.3.2.2.9.4.10.2.6  PROCESSING 

The  following  describes  the  processing  performed  by  this  part: 

function  Integrate  (East_Rho  :  Angular  Velocities; 

DeltaJTime  :  Intervals  :=  Default_Delta_Time) 
return  Earth  Positions  is 


4V«*V  A  L> 
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Latitude  :  Earth_Positions; 
begin 

Latitude  :=  Previous_Latitude  + 

((East_Rho  -  Previous_East_Rho)  *  (0.5  *  DeltaJTime)  ); 

Previous_Latitude  :=  Latitude; 

Previous_East_Rho  :=  East_Rho; 

return  Latitude; 

end  Integrate; 


3.3.2.2.9.4.10.2.7  UTILIZATION  OF  OTHER  ELEMENTS 
UTILIZATION  OF  ANCESTRAL  ELEMENTS: 

The  following  tables  describe  the  elements  used  by  this  part  but  defined  in  one 
or  more  ancestral  units: 

Data  types: 

The  following  table  summarizes  the  types  required  by  this  part  and  defined  as 
generic  formal  types  in  the  package  specification  for  Latitude_Integration: 


|  Name 

1  Type  | 

Description  | 

|  Angular_  |  floating  j 

|  Velocities  j  point  type  | 

|  Earth_Positions  j  floating  | 

j  j  point  type  j 

j  Intervals  |  floating  i 

|  |  point  type  j 

Data  type  of  angular  velocity  objects  | 

1 

Data  type  of  latitude  and  longitude  values | 

1 

Data  type  of  time  values  j 

1 

Data  objects: 

The  following  table  describes  the  data  objects  required  by  this  part  and 
maintained  local  to  the  Latitude_Integration  package. 

|  Name  | 

Type  |  Value 

|  Description  | 

|  Previous_  | 
j  East_Rho  j 
j  Previous_  j 
j  Latitude  j 

Angular_  |  N/A 

Velocities  | 

Earth_  j  N/A 

Positions  j 

|  Previous  value  of  the  East  component  | 
j  of  the  Earth-relative  rotation  j 
j  Previous  value  of  the  latitude  j 

1  1 

Subprograms  and  task  entries: 

The  following  describes  the  subprograms  required  by  this  part  and  defined  as 
generic  formal  subprograms  to  the  Latitude_Integration  package: 
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|  Name 

1  Type 

|  Description 

1 

|  »*» 

1 

|  function 

1 

|  Multiplication  operator  defining  the  operation: 
j  Angular_Velocities  *  Intervals  :=  Earth_Positions 

1 

I 

3.3.2.2.9.4.10.2.8 

LIMITATIONS 

None. 


3. 3. 2. 2. 9. 5  LONGITUDE  INTEGRATION  PACKAGE  DESIGN  (CATALOG  #P264-0) 

This  LLCSC,  which  is  a  package,  calculates  the  missile's  current  longitude 
using  trapezoidal  integration.  This  part  can  be  used  when  a  north  pointing 
local  level  coordinate  system  is  being  used  for  navigation. 

The  decomposition  for  this  part  is  the  same  as  that  shown  in  the  Top-Level 
Design  Document. 


3. 3. 2. 2. 9. 5.1  REQUIREMENTS  ALLOCATION 
This  part  meets  CAMP  requirement  R031. 


3. 3. 2. 2. 9. 5. 2  LOCAL  ENTITIES  DESIGN 
None. 


3. 3. 2. 2. 9. 5. 3  INPUT/OUTPUT 
GENERIC  PARAMETERS: 

Previously  defined  in  package  specification  for  North_Pointing_Navigation_Parts 
Data  types: 

The  following  table  describes  the  generic  formal  types  required  by  this  part: 


|  Name  |  Type  |  Description 


Angular_ 

Velocities 

Earth_Positions 

Intervals 

Sin  Cos  Ratio 


floating 
point  type 
floating 
point  type 
floating 
point  type 
floating 
point  type 


Data  type  of  angular  velocity  objects 

Data  type  of  latitude/longitude  values 

Data  type  of  time  values 

Data  type  of  values  return  by  sin/cos 
functions 


:1 

i 
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Data  objects: 

The  following  table  describes  the  generic  formal  objects  required  by  this  part: 


Default_ 
Delta_ 
Initial_ 
North  Rho 


Initial_ 
Latitude 
Initial_ 
Longi tude 


Type 

Intervals 


Angular_ 

Velocities 


Earth_ 

Positions 

Earth_ 

Positions 


Value  |  Description 
N/A  I  Default  time 


Default  time  over  which  integration  is 
to  take  place 

Initial  value  of  the  North  component  of 
the  earth-relative  rotation  rate  of  the 
navigation  coordinate  system 
Initial  value  of  the  latitude 


Initial  value  of  the  longitude 


Subprograms : 


The  following  table  describes  the  generic  formal  subroutines  required  by  this 
part: 


Name 

i  Type 

ft fW 

function 

function 

Cos 

function 

Description 


Division  operator  defining  the  operation: 
Angular_Velocities  /  Sin_Cos_Ratio  := 
Angular~Veloci ties 

Multiplication  operator  defining  the  operation: 
Angular_Velocities  *  Intervals  :*  Earth_Positions 
Cosine  function 


3. 3. 2, 2. 9. 5, 4  LOCAL  DATA 
Data  objects: 

The  following  table  describes  the  data  objects  maintained  local  to  this  part: 


|  Name 

Type 

Description  | 

|  Previous_ 

j  Integration_Variable 
|  Previous_ 
j  Longitude 

Angular_ 

Velocities 

Earth_ 

Positions 

Previous  value  of  the  integration  | 
variable  j 
Previous  longitude  value  i 

1 

3. 3. 2. 2. 9. 5. 5  PROCESS  CONTROL 
Not  applicable. 


" VK  j/v 
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3. 3. 2. 2. 9. 5. 6  PROCESSING 

The  following  describes  the  processing  performed  by  this  part: 
package  body  Longitude__Integration  is 

— local  variables- 

Previous_Integration_Variable  :  Angular  Velocities 

:=  InitIal_North_Rho  / 

Cos ( Ini t i al_Lat i tude ) ; 

Previous_Longitude  :  Earth_Positions  :  =  Ini tial_Longi tude; 

end  Longitude_Integration; 

3. 3. 2. 2. 9. 5. 7  UTILIZATION  OP  OTHER  ELEMENTS 
None. 

3. 3. 2. 2. 9. 5. 8  LIMITATIONS 
None. 

3. 3. 2. 2. 9. 5. 9  LLCSC  DESIGN 
None. 

3.3.2.2.9.5.10  UNIT  DESIGN 

3.3.2.2.9.5.10.1  REINITIALIZE  UNIT  DESIGN 

This  unit,  which  is  a  procedure,  reinitializes  the  previous  integration 
variable  and  the  previous  longitude. 

3.3.2.2.9.5.10.1.1  REQUIREMENTS  ALLOCATION 
This  part  partially  meets  CAMP  requirement  R031. 

3.3.2.2.9.5.10.1.2  LOCAL  ENTITIES  DESIGN 
None. 

3.3.2.2.9.5.10.1.3  INPUT/OUTPUT 


’JvTVJ 


Sr 


S*; 


LV 


Av 


FORMAL  PARAMETERS: 
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The  following  table  describes  this  part's  formal  parameters: 


|  Name  |  Type 


|  Mode  |  Description 


North  Rho 


Angular_ 

Velocities 


In 


Latitude  j  Earth_Positions  j  In 
Longitude  j  Earth  Positions  j  In 


Initial  value  of  the  North 
component  of  the  earth-relative 
rotation  rate  of  the  navigation 
coordinate  system 
Initial  value  of  the  latitude 
Initial  value  of  the  longitude 


3.3.2.2.9.5.10.1.4  LOCAL  DATA 
None. 

3.3.2.2.9.5.10.1.5  PROCESS  CONTROL 
Not  applicable. 

3.3.2.2.9.5.10.1.6  PROCESSING 

The  following  describes  the  processing  performed  by  this  part: 

procedure  Reinitialize  (North_rho  :  in  Angular_Velocities; 

Latitude  :  in  Earth_Positions; 

Longitude  :  in  EarthPositions)  is 

begin 

Previous_Integration_Variable  :=  North_Rho  /  Cos(Latitude) ; 

Previous_Longitude  :=  Longitude; 

end  Reinitialize; 


3.3.2.2.9.5.10.1.7  UTILIZATION  OF  OTHER  ELEMENTS 
UTILIZATION  OF  ANCESTRAL  ELEMENTS: 


The  following  tables  describe  the  elements  used  by  this  part  but  defined  in  one 
or  more  ancestral  units: 

Data  types: 

The  following  table  summarizes  the  types  required  by  this  part  and  defined  as 
generic  formal  parameters  to  the  Longitude_Integration  package: 
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|  Name 


Type  |  Description 


Angular_ 

Velocities 

Earth_Positions 

Sin  Cos  Ratio 


floating 
point  type 
floating 
point  type 
floating 
point  type 


Data  type  of  angular  velocity  objects 

Data  type  of  latitude/longitude  values 

Data  type  of  values  return  by  sin/cos 
functions 


Data  objects: 

The  following  objects  are  required  by  this  part  and  maintained  in  the  package 
body  of  Longitude_Integration: 


|  Name 

Type 

Description  | 

|  Previous 

j  Integration  Variable 
|  Previous 
j  Longitude 

Angular 

Velocities 

Earth 

Positions 

Previous  value  of  the  integration  | 
variable  j 
Previous  longitude  value  j 

1 

Subprograms  and  task  entries: 

The  following  table  summarizes  the  subroutines  and  task  entries  required  by 
this  part  and  defined  as  generic  formal  parameters  to  the  Longitude_Integration 
package : 


|  Name 

1  Type  j 

|  Description  | 

|  "/" 

|  function 

|  Division  operator  defining  the  operation:  j 

1 

i  I 

Angular  Velocities  /  Sin  Cos  Ratio  :=  i 

1 

1  1 

|  Angular  Velocities  j 

|  Cos 

|  function  | 

Cosine  function  j 

3.3.2.2.9.5.10.1.8  LIMITATIONS 


None. 


3.3.2.2.9.5.10.2  INTEGRATE  UNIT  DESIGN 

This  unit,  which  is  a  function,  calculates  the  new  longitude  using  trapezoidal 
integration. 


3.3.2.2.9.5.10.2.1  REQUIREMENTS  ALLOCATION 
This  unit  partially  meets  CAMP  requirement  R031. 


CAMP  Software  Detailed  Design  Document 

3.3.2.2.9.5.10.2.2  LOCAL  ENTITIES  DESIGN 
None. 

3.3.2.2.9.5.10.2.3  INPUT/OUTPUT 
FORMAL  PARAMETERS: 

The  following  table  describes  this  part's  formal  parameters: 
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|  Name 

1  Type 

|  Mode 

|  Description 

1 

|  North  Rho 

|  Angular 

1  In 

|  Current  value  of  the  North 

i 

1  Velocities 

1 

j  component  of  the  earth-relative 

1 

1 

i 

|  rotation  rate  of  the  navigation 

1 

1 

1 

j  coordinate  system 

1 

|  Latitude 

j  Earth  Positions 

1  In 

j  Current  latitude 

1 

j  Delta  Time 

|  Intervals 

1  In 

j  Time  interval  over  which  the 

1 

1 

j  integration  is  to  take  place 

1 

3.3.2.2.9.5.10.2.4  LOCAL  DATA 

Data  objects: 

The  following  table  describes  the  local  data  maintained  by  this  part: 


|  Name 

Type 

Value 

|  Description  | 

|  Longitude 
j  Temp  PIV 

1 

Earth_Positions 

Angular_ 

Velocities 

N/A 

N/A 

|  Longitude  being  calculated  | 
j  Temporary  previous  integration  j 
j  variable  j 

3.3.2.2.9.5.10.2.5  PROCESS  CONTROL 
Not  applicable. 

3.3.2.2.9.5.10.2.6  PROCESSING 

The  following  describes  the  processing  performed  by  this  part: 

function  Integrate  (North _Rho  :  Angular_Velocities; 

Latitude  :  Earth_Positions; 

Delta_Time  :  Intervals  :=  Default_Delta_Time) 
return  Earth  Positions  is 


— declaration  of  variables- 
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Longitude  :  Ear th_Posit ions; 
Temp_PIV  :  Angular_Velocities; 


— begin  function  Integrate 


begin  i 

Temp_PIV  North_Rho  /  Cos(Latitude);  j 

Longitude  :=  Previous_Longitude  +  ; 

((Temp_PIV  -  Previous_Integration_Variable)  *  | 

(0.5  *  Delta_Time));  | 

| 

Previous_Integration_Variable  :=  Temp_PIV;  • 

Previous_Longitude  :«  Longitude;  \ 

A 

$ 

return  Longitude;  l 

•i 

end  Integrate;  j 


3.3.2.2.9.5.10.2.7  UTILIZATION  OF  OTHER  ELEMENTS 
UTILIZATION  OF  ANCESTRAL  ELEMENTS: 


,1 

The  following  tables  describe  the  elements  used  by  this  part  but  defined  in  one  | 

or  more  ancestral  units:  5 

\ 

Data  types:  i 

The  following  table  summarizes  the  types  required  by  this  part  and  defined  as  j 

generic  formal  parameters  to  the  Longitude_Integration  package:  j 


J 

i 

j 

.j 

3 

) 

j 


Name 


Type 


Description 

Data  type  of  angular  velocity  objects 

Data  type  of  latitude/longitude  values 

Data  type  of  time  values 

Data  type  of  values  return  by  sin/cos 
functions 


Angular_ 

Velocities 

Earth_Positions 

Intervals 

Sin  Cos  Ratio 


|  floating 
I  point  type 
j  floating 
|  point  type 
j  floating 
j  point  type 
j  floating 
j  point  type 


Data  objects: 

The  following  objects  are  required  by  this  part  and  maintained  in  the  package 
body  of  Longitude  Integration: 
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j  Name 

Type 

Description 

1 

|  Previous 

Angular 

Previous  value  of 

the  integration  | 

j  Integration  Variable 

Velocities 

variable 

i 

j  Previous 

Earth 

Previous  longitude 

value  j 

j  Longi tude 

Positions 

1 

Subprograms  and  task  entries: 

The  following  table  summarizes  the  subroutines  and  task  entries  required  by 
this  part  and  defined  as  generic  formal  parameters  to  the  Longitude_Integration 
package: 


Name 

1  Type 

ft  /!? 

|  function 

1 

ii*n 

|  function 
| 

Cos 

1 

|  function 

Division  operator  defining  the  operation: 
Angular_Velocities  /  Sin_Cos_Ratio  := 
Angular_Velocities 

Multiplication  operator  defining  the  operation: 
Angular_Velocities  *  Intervals  :=  Ear th_Posit ions 
Cosine  function 


3.3.2.2.9.5.10.2.8  LIMITATIONS 


None. 


3.3.2.2.10  UNIT  DESIGN 

3.3.2.2.10.1  COMPUTE  CORIOLIS  ACCELERATION  UNIT  DESIGN  (CATALOG  #P258-0) 

This  function  computes  the  components  of  the  coriolis  acceleration  vector  when 
a  local  level,  north  pointing  coordinate  system  is  being  used  for  navigation. 

The  calculations  performed  are  as  follows: 

CA  :=  (2  *  Omega_Vector  +  Rho_Vector)  x  Velocity_Vector 


3.3.2.2.10.1.1  REQUIREMENTS  ALLOCATION 
This  part  meets  CAMP  requirement  R008. 


3.3.2.2.10.1.2  LOCAL  ENTITIES  DESIGN 


None. 


i 


SSrA 
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3.3.2.2.10.1.3  INPUT/OUTPUT 
GENERIC  PARAMETERS: 

The  following  generic  parameters  were  previously  defined  in  package 
specificatio  .  for  North_Pointing_Navigation_Parts. 

Data  types: 

The  following  table  describes  the  generic  formal  types  required  by  this  part: 


|  Name  |  Type  j  Description 


|  Velocity_Vectors  |  private  |  Array  of  velocities  | 
j  Angular_Velocity_  |  private  |  Array  of  angular  velocities  j 
j  Vectors  j  j  j 
j  Acceleration_Vectors j  private  |  Array  of  accelerations  j 


Subprograms : 

The  following  table  describes  the  generic  formal  subroutines  required  by  this 
part: 


Description 


Defines  the  way  two  angular  velocity 
vectors  are  added  when  the  first 
component  in  the  second  vector  equals 
0 

Performs  a  cross  product  operation 
between  an  angular  velocity  vector  and 
a  velocity  vector,  resulting  in  an 
acceleration  vector 


FORMAL  PARAMETERS: 

The  following  table  describes  this  part's  formal  parameters: 
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3.3.2.2.10.1.4  LOCAL  DATA 
Data  objects: 

The  following  table  describes  the  data  objects  maintained  by  this  part: 


!  Name 


Description 


Coriolis_ 
Acceleration 
Temp  AW 


Acceleration 
Vectors 
Angular_ 
Veloci ty_ 
Vectors 


This  is  the  value  being  calculated  and 
returned 

Temporary  angular  velocity  vector  used  for 
intermediate  calculations 


3.3.2.2.10.1.5  PROCESS  CONTROL 
Not  applicable. 


3.3.2.2.10.1.6  PROCESSING 

The  following  describes  the  processing  performed  by.  this  part: 

function  Compute_Coriolis_Acceleration 

(Veloci ty_In  :  Veloci ty_Vec tors; 

Rho_In  :  Angular_Velocity_Vectors; 

0mega_In  :  Angular_Velocity_Vectors) 
return  Acceleration  Vectors  is 


— declaration  of  variables- 


Coriolis_Acceleration  :  Acceleration_Vectors; 
Temp_AW  :  Angular_Velocity_Vectors; 


- beginning  of  function- 


begin 

Temp_AW  :=  Sparse_Right_X_Add(Omega_In,  0mega_In); 

Temp_AW  :=  Sparse_Right_X~Add(Rho_In,  Temp_AW); 

Coriolis_Acceleration  :=  Cross_Product(Temp_AW,  Veloci ty_In); 


return  Coriolis_Acceleration; 
end  Compute_Coriolis_Acceleration; 
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3.3.2.2.10.1.7  UTILIZATION  OF  OTHER  ELEMENTS 
None. 

3.3.2.2.10.1.8  LIMITATIONS 
None. 

3.3.2.2.10.2  TOTAL  J>LATF0RM_R0TATI0N_RATES  UNIT  DESIGN  (CATALOG  #P260-0) 

This  function  computes  the  rotation  rate  of  the  navigation  coordinate  system 
with  respect  to  inertial  space. 

The  calculations  performed  are  as  follows: 

TPR  :=  Rho_Vector  +  Omega_Vector 

3.3.2.2.10.2.1  REQUIREMENTS  ALLOCATION 
See  top  header. 

3.3.2.2.10.2.2  LOCAL  ENTITIES  DESIGN 
None. 


3.3.2.2.10.2.3  INPUT/OUTPUT 
GENERIC  PARAMETERS: 

The  following  generic  parameters  were  previously  defined  when  this  part  was 
specified. 

Data  types: 

The  following  table  describes  the  generic  formal  types  required  by  this  part: 


|  Name  | 

Type 

j  Description  | 

|  Angular_Velocity_Vectorsj 

private 

|  Array  of  angular  velocities  | 

Subprograms : 

The  following  table  describes  the  generic  formal  subroutines  required  by  this 
part: 
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|  Name  i 

1  Type 

|  Description  | 

|  Sparse  Right  X  Add  | 
1  "  ! 
1  1 

|  function 

|  Adds  two  angular  velocity  vectors  assuming  | 

J  the  x-component  of  the  right  vector  equals j 

1  o.o  | 

FORMAL  PARAMETERS: 

The  following  table  describes  this  part's  formal  parameters: 


|  Name  |  Type  |  Mode  |  Description 


Rho  Vector 


Omega  Vector 


Angular_ 

Velocity_ 

Vectors 

Angular_ 

Velocity 

Vectors 


Rotation  rate  vector  of  the  navigation 
coordinate  system  with  respect  to  the 
earth 

Earth's  rotation  rate  vector 


3.3.2.2.10.2.4  LOCAL  DATA 
None. 


3.3.2.2.10.2.5  PROCESS  CONTROL 
Not  applicable. 


3.3.2.2.10.2.6  PROCESSING 

The  following  describes  the  processing  performed  by  this  part: 

function  Total  Platform_Rotation  Rates 

(RKo_Vector  :  AnguIar_Velocity_Vectors; 
Omega_Vector  :  Angular_Velocity  Vectors) 
return  Angular_Velocity_Vectors  Ts 

begin 

return  Sparse_Right_X_Add  (Left  =>  Rho_Vector, 

Right  =>  Omega_Vector); 
end  Total_Platform  Rotation  Rates; 


3.3.2.2.10.2.7  UTILIZATION  OF  OTHER  ELEMENTS 
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3.3.2.2.10.2.8  LIMITATIONS 


None. 
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package  body  North_Pointing_Navigation_Parts  is 
pragma  PAGE; 

function  Compute_Coriolis_Acceleration 

(Velocity_In  :  Velocity_Vectors; 

Rho_In  :  Angular_Velocity_Vectors; 

Omega_In  :  Angular_Velocity_Vectors) 
return  Acceleration  Vectors  is 


—  declaration  of  variables- 


Coriolis_Acceleratio'.i  :  Acceleration_Vectors; 
Temp_Avv  :  Angular_Velocity_Vectors: 


—  —  beginning  of  function 


begin 

Temp_Avv  : =  Sparse_Right_X_Add(Omega_In,  Omega_In); 

Temp_Avv  s=  Sparse~Right_X_Add(Rho_In,  Temp_Avv); 

Coriolis_Acceleration  :=  Cross_Product(Temp_Avv,  Velocity_In) ; 

return  CoriolisAcceleration; 

end  Compute_Coriolis_Acceleration; 

pragma  PAGE; 

package  body  Radius_Of_Curvature  is 


—  local  declarations 


E 

N 

V 

pragma  PAGE; 


—  unit  body 


function  Compute  (Latitude  :  Earth_Positions; 

Altitude  :  Distances) 
return  Distance  Vectors  is 


—  declaration  of  variables- 


Cos  Of_Lat  :  Sin  Cos_Ratio; 

Flat_X_Sin_Squared  :  Real; 
Long_Denominator  :  Real; 


:  constant  Indices  :=  Indices' FIRST; 

:  constant  Indices  :=  Indices' SUCC(E) ; 
:  constant  Indices  :=  Indices' LAST; 
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Radius_Of_Curvature  :  Distance_Vectors; 
Short_Denominator  :  Real; 

Sin  Of  Lat  :  Sin  Cos  Ratio; 


—  begin  function  Compute 


begin 


Sin_Cos(Latitude,  Sin_0f_Lat,  Cos_0f_Lat); 


Flat_X_Sin_Squared  :=  Earth_Flattening_Coefficient  * 

<Sin_0f_Lat  *  Sin_0f_Lat); 


Short_Denominator  :=  1.0  - 

Altitude  *  0ne_0ver_Earth_Radius  - 
Flat  X  Sin  Squared; 


Long_Denominator  :=  Short_Denominator  + 

2.0  *  Earth_Flattening_Coefficient  * 
(Cos  Of  Lat*Cos  Of  Lat); 


Radius_Of_Curvature(E)  :=•  Earth_Radius  /  Short_Denominator; 
Radius_Of_Curvature(N)  :*  Earth_Radius  /  Long_Denominator; 
Radius_Of_Curvature(V)  :=  Earth_Radius  *  (1.0  -  Flat_X_Sin_Squared)  + 
~  Altitude; 


return  Radius  Of  Curvature; 


end  Compute; 


end  Radius  Of  Curvature; 


pragma  PAGE; 

function  Total  Platforra_Rotation  Rates 

(REo_Vector  :  AnguIar_Velocity_Vectors; 
Omega_Vector  ;  Angular_Velocity  Vectors) 
return  Angular_Velocity_Vectors  is 

begin 

return  Sparse_Right_X_Add  (Left  =■>  Rho_Vector, 

Right  =>  Omega_Vector) ; 
end  Total  Platform  Rotation  Rates; 


pragma  PAGE; 

package  body  Earth_Rotation_Rate  is 


—  declaration  of  variables- 


E  :  constant  Indices  :=  Indices' FIRST; 

N  :  constant  Indices  :=  Indices' SUCC(E); 
V  :  constant  Indices  :=  Indices'LAST; 


pragma  PAGE; 
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—  unit  body 


function  Compute  (Latitude  :  Earth_Positions) 

return  Angular_Velocity_Vectors  is 


—  declaration  of  variables- 


Cos_0f_Lat  :  Sin_Cos_Ratio; 
Rotation_Rate  :  Angular_Velocity_Vectors; 
Sin  Of  Lat  :  Sin  Cos  Ratio; 


— beginning  of  function- 


begin 

Sin_Cos(Latitude,  Sin_0f_Lat,  Cos_0f_Lat); 
Rotation_Rate(E)  :=  0.0; 

RotationRate(N)  ;=*  Earth_Rate  *  Cos_0f_Lat; 
Rotation_Rate(V)  :«  Earth~R*te  *  Sin_0f_Lat; 

return  RotationRate; 

end  Compute; 

end  Earth_Rotation_Rate; 
pragma  PAGE; 

package  body  Earth_Relative_Navigation_Rotation_Rate  is 


— declaration  of  variables- 


E  :  constant  Indices  :=  Indices' FIRST; 

N  :  constant  Indices  :=*  Indices' SUCC(E) ; 

V  :  constant  Indices  :=  Indices' LAST; 

pragma  PAGE; 


— unit  body 


function  Compute  (Missile_Velocity  :  Velocity_Vectors; 

Radius_Of_Curvature  :  Distance~Vectors; 
Latitude  :  Earth_Positions) 
return  Angular_Velocity_Vectors  is 


—  declaration  of  variables- 
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Rho  :  Angular_Velocity_Vectors; 


— beginning  of  funclion- 


begin 

Rho(E)  :=  -  Missile_Velocity(N)  /  Radius_Of_Curvature(N) ; 
Rho(N)  :=  Missile_Velocity(E)  /  Radius_Of_Curvature(E) ; 
Rho(V)  :=  Rho(N)  *  Tan(Latitude) ; 

return  Rho; 

end  Compute; 

end  Ear  t h_Rela t i ve_Nav i ga t i on_Ro  t a t i on_Ra  t  e ; 
pragma  PAGE; 

package  body  Latitude  Integration  is 


—  local  declarations 


Previous_East_Rho  :  Angular_Velocities  :=»  Initial_East_Rho; 
Previous_Latitude  ;  Earth_Positions  :*  Initial_Latitude; 

pragma  PAGE; 

procedure  Reinitialize  (East_Rho  :  in  AngularVelocities; 

Latitude  :  in  Earth  Positions)  is 


begin 

Previous__East_Rho  :=  East_Rho; 

Previous~Latitude  :=  Latitude; 

end  Reinitialize; 

pragma  PAGE; 

function  Integrate  (East_Rho  :  Angular  Velocities; 

Delta_Time  ;  Intervals  :=  Default_Delta_Time) 
return  Earth_Positions  is 

Latitude  :  Earth_Positions; 

begin 

Latitude  :=  Previous  Latitude  + 

(<East_RHo  -  Previous_East_Rho)  *  (0.5  *  Delta_Time)  ); 

Previous_Latitude  :=  Latitude; 

Previous”East_Rho  : =  East_Rho; 

retuim  Latitude; 

end  Integrate; 


7 


V 

1 

s 


1 


I 


I 


CAMP  Software  Detailed  Design  Document 


Page  135 


end  Latitude_Integration; 


pragma  PAGE; 

package  body  Longitude_Integration  is 


—  local  variables- 


Previous_Integration_Variable  :  Angular  Velocities 

:=  InitIal_North_Rho  / 

Cos (Ini tial_Lati tude) $ 

Previous_Longitude  :  Ear th_Posi t ions  :=  Ini tial_Longi tude; 

pragma  PAGE; 

procedure  Reinitialize  (North_Rho  :  in  Angular_Velocities; 

Latitude  :  in  Earth_Positions; 

Longitude  :  in  Earth_Positions)  is 

begin 

Previous_Integration_Variable  :*  North_Rho  /  Cos(Latitude); 
PreviousJLongitude  :*  Longitude; 

end  Reinitialize; 

pragma  PAGE; 

function  Integrate  (North_Rho  :  Angular_Veloeities; 

Latitude  s  Earth_Positions; 

DeltaJTime  :  Intervals  Default_Delta_Time) 
return  Earth  Positions  is 


— declaration  of  variables- 


Longitude  :  Earth_Positions; 
Temp_Piv  :  Angular_Velociti.es; 


— begin  function  Integrate 


begin 


i 

3 
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Temp_Piv  :=  North_Rho  /  Cos (Lati tude) ; 


Longitude  :=  Previous_Longitude  + 

((Temp_Piv  -  Previous_Integration_Variable)  * 
(0.5  *  Delta  Time)); 


Previous_Integration_Variable  :=  Temp  Piv; 
Previous  Longitude  "*  :=  Longitude; 


return  Longitude; 
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3. 3. 2. 3  WANDER  AZIMUTH  NAVIGATION  PARTS  (PACKAGE  BODY)  TLCSC  P002  (CATALOG 
#P234-0) 

This  package  contains  the  specifications  for  all  parts  which  can  be  used  in  a 
Wander  Azimuth  navigation  coordinate  system  environment. 

The  decomposition  for  this  part  is  the  same  as  that  shown  in  the  Top-Level 
Design  Document. 


3. 3. 2. 3.1  REQUIREMENTS  ALLOCATION 

The  following  chart  summarizes  the  allocation  of  CAMP  requirements  to  this 
part: 


|  Name 

Requirements  Allocation  | 

Compute  East  Velocity 

R185 

Compute  North  Velocity 

R186 

Compute  Earth  Relative  Horizontal  Velocities 

R001 

Compute  Total  Angular  Velocity 

R004 

Compute  Coriolis  Acceleration 

R007 

Coriolis  Acceleration  From  Total  Rates 

R187 

Radius  ol  Curvature 

R035 

Total  Platform  Rotation  Rate 

R011 

Earth  Rotation  Rate 

R013 

Earth  Relative  Rotation  Rate 

R025 

Latitude 

R029 

Compute  Latitude  using  Arctangent 

R030 

Compute  Longitude 

R033 

Compu  t e_Wander_Az imu  th_Angle 

R028 

3. 3. 2. 3. 2  LOCAL  ENTITIES  DESIGN 
None. 


3. 3. 2. 3. 3  INPUT/OUTPUT 
None. 

3. 3. 2. 3. 4  LOCAL  DATA 
None. 


3. 3. 2. 3. 5  PROCESS  CONTROL 
Not  applicable. 
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3. 3. 2. 3. 6  PROCESSING 


The  following  describes  the  processing  performed  by  this  part: 


package  body  Wander_Azimuth_Navigation_Parts  is 


function  Compute_East_Velocity 

(Nominal_Eas  t_Veloci ty 
Nominal_North_Veloci ty 
Current_Wander_Angle 


:  Velocities; 

:  Velocities; 

:  Wander  Angles) 


return  Velocities  is  separate; 


function  Compute_East_Velocity_with_Sin_Cos  In 

(Nominal_East_Velocity  :  Velocities? 
Nominal_North_Velocity  :  Velocities; 
Sine_of_Wander_Angle  :  Sin_Cos_Ratio? 
Cosine_of_Wander_Angle  :  Sin_Cos_Ratio) 
return  Velocities  is  separate; 


function  Compute_North_Velocity 

(Norainal_East_Velocity  :  Velocities; 
Nominal_North_Velocity  :  Velocities; 
Current._Wander_Angle  :  Wander_Angles) 
return  Velocities  is  separate; 


function  Compute_North_Velocity  with_Sin  Cos_In 
(Nominal_East  Velocity  T  Velocities; 
Nominal_NortH_Velocity  :  Velocities; 
Sine_of_Wander_Angle  :  Sin_Cos_Ratio; 
Cosine_of_Wander_Angle  :  Sin~Cos_Ratio) 
return  Velocities  is  separate; 


procedure  Compute_Earth_Relative  Horizontal_Velocities 
(Nominal_East  Velocity  :  in  Velocities; 
Nominal_Nor tE_Veloci ty 
Current  Wander_Angle 
East  Velocity 
NortK_Velocity 
is  separate; 


in  Velocities; 
in  Wander_Angi.es ; 
out  Velocities; 
out  Velocities) 


procedure  Compute  Earth_Relative  Horizontal_Velocities_with_Sin_Cos_In 
(NomInal_East  Velocity  :  in  Velocities; 
Nominal_NortK_Veloci ty 
Sine_of_Wander_Angle 
Cosine_of_Wander_Angle 
East  Velocity 
NortE_Velocity 
is  separate; 


in  Velocities; 
in  Sin_Cos_Ratio; 
in  Sin_Cos  Ratio; 
out  Velocities; 
out  Velocities) 


function  Compute_Total_Angular_Velocity 

(Rho_East  :  Angular_Velocities; 
Rho_North  :  Angular  Velocities) 
return  Angular_VelocIties  is  separate; 


function  Compute_Coriolis_Acceleration 

(Velocity_in  :  Velocity_Vectors; 

Rho_in  :  Angular_Velocity_Vectors? 


,VW:> 


‘  \V 
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Omega_in  :  Angular_Velocity_Vectors) 
return  Acceleration_Vectors  is  separate; 

package  body  Coriolis_Acceleration_From_Total_Rates  is  separate; 

function  Compute_Curvatures 

(DC_P_V  :  Direction_Cosine_Matrix_Elements; 

DC_P_NE  :  Direction_Cosine_Matrix_Elements; 

DC_P_NN  :  Direction_Cosine_Matrix_Elements; 

Current_Altitude  :  Distances) 

return  Inverse_Distance_Vectors  is  separate; 

function  Total_Platform_Rotation  Rate 

(Rho_Vector  :  AnguIar_Velocity_Vectcrs; 

Omega_Vector  :  Angular_Velocity  Vectors) 
return  Angular_Velocity_Vectors  Is  separate; 

package  body  Earth_R°tati°n_Rate  is  separate; 

function  Compute_Earth_Relative_Navigation_Rotation_Rate 
(Missile_Velocity  s  Velocity_Vectors; 

Curvatures  :  Inverse_Distance_Vectors) 

return  Angular_Velocity_Vectors  is  separate; 

function  Compute_Latitude  <Sin_Latitude  :  Sin_Cos  Ratio) 

return  Ear th_Posit ions  Is  separate; 

function  Coraput e_La t i tude  us ing_Arc t angen t 

(DC_P_V  t  Direction_Cosine_Matrix_Elements; 

DC_P_NE  :  Direction_Cosine_Matrix_Elements; 

DC_P_NN  :  Direction_Cosine_Matrix_Elements) 
return  Earth_Positions  is  separate! 

function  Compute_Latitude_using_Two_Value_Arctangent 

(DC_P_V  :  Direction_Cosine_Matrix_Elements; 

DC_P_NE  :  Direction_Cosine_Matrix_Elements; 

DC_P_NN  :  Direction_Cosine_Matrix_Elements) 
return  Ear th_Posit ions  is  separate; 

function  Compute_Longitude 

(DC_R _V  :  Direction_Cosine_Matrix_Elements; 

DC_G_V  :  Direction_Cosine_Matrix_Elements) 
return  Earth_Positions  is  separate; 

function  Compu t e_Longi tude_us ing_Tvo_Value_Ar c  t angen t 
(DC_R_V  :  Direction_Cosine_Matrix_Elements; 

DC_G_V  :  Direction_Cosine_Matrix_Elements) 
return  Earth_Positions  is  separate; 

function  Compute_Wander_Azimuth_Angle 

(DC_P_NE  :  Direction_Cosine_Matrix_Elements; 

DC_P_NN  :  Direction_Cosine_Matrix_Elements) 
return  Wander_Angles  is  separate; 

function  Compu te_Vander_Azimuth_Angle_using_Tvo_Value_Arc tangent 
(DC_P_NE  :  Direction_Cosine_Matrix_Eleraents; 

DC_P_NN  :  Direction_Cosine_Matrix  Elements) 


■Vv 
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return  Wander_Angles  is  separate; 
end  Wander  Azimuth_Navigation_Parts; 


3. 3. 2. 3. 7  UTILIZATION  OF  OTHER  ELEMENTS 
None. 

3. 3. 2. 3. 8  LIMITATIONS 
None. 

3. 3. 2. 3. 9  LLCSC  DESIGN 

3. 3. 2. 3. 9.1  CORIOLIS  ACCELERATION_FROM_TOTAL  RATES  PACKAGE  DESIGN  (CATALOG 
#P240-0) 

This  package  contains  a  function  which  computes  the  Coriolis  acceleration  given 
the  velocity  vector,  Rho  vector,  and  Total  Rates  vector. 

The  computations  are  performed  as  follows: 

(2  *  Omega  Vector  +  Rho  Vector)  x  Velocity  Vector 

The  decomposition  for  this  part  is  the  same  as  that  shown  in  the  Top-Level 
Design  Document. 


3. 3. 2. 3. 9. 1.1  REQUIREMENTS  ALLOCATION 
This  part  meets  CAMP  requirement  R187. 


3. 3. 2. 3. 9. 1.2  LOCAL  ENTITIES  DESIGN 
None. 


3. 3. 2. 3. 9. 1.3  INPUT/OUTPUT 
GENERIC  PARAMETERS: 

The  following  generic  parameters  for  this  part  were  previously  defined  in  the 
package  specification  of  Wander_Azimuth_Navigation_Parts: 

Data  types: 

The  following  table  describes  the  generic  formal  types  required  by  this  part: 


□rvr. 

r>. 

jl  r_M. 
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|  Name 


|  Description 


Acceleration_Vectors 
Angular  Velocities 


Indices 


Angular_Velocity_Vectors 
Velocity  Vectors 


private 
floating 
point  type 
discrete 
type 
array 
private 


Array  of  acceleration  measurements 
Data  type  defining  angular  velocity 
measurements 

Used  to  dimension  Angular_Velocity_ 
Vectors 

Array  of  angular  velocities 
Array  of  velocity  measurements 


Subprograms : 


The  following  table  describes  the  generic  formal  subroutines  required  by  this 
part: 


|  Name 

Type 

Description  | 

|  Cross  Product 

1 

1 

function 

Cross  product  function  performing  a  cross  j 

product  operation  on  Angular  Velocity  Vectors  j 
and  Velocity_Vectors  to  obtain  Acceleration  j 

Vectors  ~  j 

FORMAL  PARAMETERS: 


.The  following  table  describes  the  formal  parameters  to  the  unit  contained  in 
this  part: 


|  Mode  |  Description 


Velocity_in 

1 

Velocity 

In 

1 

Vectors 

Omega 

1 

1 

Angular_ 

In 

1 

1 

Velocity_Vectors 

Total_Rates 

Angular_ 

In 

1 

Veloci ty_Vectors 

Contains  the  nominal  East,  North 
and  vertical  components  of  the 
missile's  velocity 
Contains  the  nominal  East,  North 
and  vertical  components  of  the 
missile's  velocity 
Contains  the  nominal  East,  North 
and  vertical  components  of  the 
total  platform  rate 


3. 3. 2. 3-9. 1.4  LOCAL  DATA 


Data  objects: 

The  following  table  describes  the  data  objects  maintained  by  this  part: 
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|  Name  |  Type  | 

Value 

|  Description  | 

j  E  |  Indices  j 

j  N  j  Indices  j 

j  U  j  Indices  j 

Indices 'FIRST 
Indices' SUCC(E) 
Indices' LAST 

|  Index  into  first  element  of  array  | 
j  Index  into  second  element  of  array  j 
j  Index  into  last  element  of  array  j 

The  following  table 
in  this  part: 

describes  the  data  objects  maintained  by  the  unit  contained 

|  Name  |  Type 

|  Value 

|  Description  | 

|  Temp  A  V  V  |  Angular_  |  N/A 
|  ~  ’  |  Velocity  | 

j  j  Vectors  j 

|  Temporary  angular  velocity  vector  | 

1  '  1 

1  1 

3. 3. 2. 3. 9. 1.5  PROCESS  CONTROL 
Not  applicable. 


3. 3. 2. 3. 9. 1.6  PROCESSING 

The  following  describes  the  processing  performed  by  this  part: 


separate  (Wander  Azimuth_Navigation  Parts) 

package  body  CorIolis_Acceleration_?rom_Total_Rates  is 


- declaration  of  variables- 


E 

N 

U 


:  constant  Indices  :=  Indices' FIRST; 

:  constant  Indices  :=  Indices' SUCC(E) ; 
:  constant  Indices  :  =  Indices' LAST; 


- unit  body- 


function  Compute  (Velocity_In  :  Velocity_Vectors; 

Omega  :  Angular_Velocity_Vectors; 
Total_Rates  :  Angular_Velocity_Vectors) 
return  Acceleration  Vectors  is 


— declaration  of  variables- 


Temp  A  V  V 


:  Angular_Velocity_Vectors; 


i  1&. 
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—  — begin  function  Compute 
begin 

Temp_A_V_V(E)  :=  Omega(E)  +  Total_Rates(E); 

Temp_A_V_V(N)  :=  Omega(N)  +  Total_Rates(N); 

Temp_A_V_V(U)  :=  Omega(U)  +  Omega(U); 

return  Cross_Product(Temp_A_V_V,  Velocity_In); 

end  Compute; 

end  Coriolis  Acceleration  From  Total  Rates; 


3. 3. 2. 3. 9. 1.7  UTILIZATION  OF  OTHER  ELEMENTS 
None. 


3. 3. 2. 3. 9. 1.8  LIMITATIONS 


None. 


3. 3. 2. 3. 9. 1.9  LLCSC  DESIGN 


None.* 


3.3.2.3.9.1.10  UNIT  DESIGN 


None. 


3. 3. 2. 3. 9. 2  EARTH_ROTATION_RATE  PACKAGE  DESIGN  (CATALOG  #P243-0) 

This  package  contains  a  function  which  computes  the  rotation  rate  of  the  Earth 
given  some  elements  of  the  Direction  Cosine  matrix. 

The  computations  are  performed  as  follows: 

Omega(i)  :=  Direction  Cosine  Matrix(P,i)  *  Earth's  rotation  rate 

The  decomposition  for  this  part  is  the  same  as  that  shown  in  the  Top-Level 
Design  Document. 


3. 3. 2. 3. 9. 2.1  REQUIREMENTS  ALLOCATION 
This  part  meets  CAMP  requirement  R013. 
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3. 3. 2. 3. 9. 2. 2  LOCAL  ENTITIES  DESIGN 
None. 


3. 3. 2. 3. 9. 2. 3  INPUT/OUTPUT 
GENERIC  PARAMETERS: 

The  following  generic  parameters  were  previously  defined  in  the  package 
specification  of  Vander_Azimuth_Navigation_Parts: 

Data  types: 

The  following  table  describes  the  generic  formal  types  required  by  this  part: 


|  Name  |  Type  |  Description 


Angular  Velocities  | 

floating 

Data  type  defining  angular  velocity 

1 

point  type 

measurements 

Indices  j 

discrete 

Used  to  dimension  angular  velocity 

type 

vector 

Angular  Velocity  Vectors | 

array 

Array  of  angular  velocities 
dimensioned  by  Indices  type 

Direction  Cosine  j 

floating 

Used  to  define  elements  of  the 

Matrix_Elements  j 

point  type 

direction  cosine  matrix 

Data  objects: 

The  following  table  describes  the  generic  formal  objects  required  by  this  part: 


|  Name 

1  Type 

|  Value 

|  Description  | 

|  Earth_Rate 

|  Angular_Velocities 

|  N/A 

|  Rotation  rate  of  the  Earth  | 

Subprograms: 

The  following  table  describes  the  generic  formal  subroutines  required  by  this 
part: 


|  Name 

1  Type 

Description  j 

|  »*" 

1 

|  function 

1 

1 

Multiplication  operator  defining  the  operation:  j 
Direction_Cosine_Matrix_Elements  *  j 
Angular_Velocities  =>  Angular_Velocities  j 

FORMAL  PARAMETERS: 

The  following  table  describes  the  formal  parameters  to  the  unit  in  this  part: 
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|  Name  |  Type 


Mode  |  Description 


DC_P_V 
DC  P  NE 


Direction_Cosine_  |  In 
Matrix_Elements  j 
Direction_Cosine_  j  In 
Matrix  Elements  I 


DC  P  NN 


Direction_Cosine 
Matrix  Elements 


In 


Sine  of  the  current  latitude  of  the 
missile 

Product  of  the  sine  of  the  wander 
angle  and  the  cosine  of  the 
latitude 

Product  of  the  cosine  of  the  wander 
angle  and  the  cosine  of  the 
latitude 


3. 3. 2. 3. 9. 2. 4  LOCAL  DATA 
Data  objects: 

The  following  table  describes  the  data  objects  maintained  by  this  part: 


|  Name 

Type 

Value 

|  Description  | 

|  NE 

Indices 

Indices' FIRST 

|  Index  into  first  element  of 

j  array 

|  NN 

Indices 

Indices 

|  Index  into  second  element  of 

' SUCC(NE) 

|  array 

1  v 

Indices 

Indices' LAST 

j  Index  into  third  element  of 

1 

]  array 

The  following  table  describes 

the  data  objects  maintain  by  the  unit  in  this 

part: 

|  Name 

Type 

Value 

|  Description  | 

|  Rotation  Rate 

Angular 

N/A 

|  Vector  being  calculated  and  | 

1 

Velocity 

i  returned  j 

1 

Vectors 

1  1 

3. 3. 2. 3. 9. 2. 5  PROCESS  CONTROL 


Not  applicable. 


3. 3. 2. 3. 9. 2. 6  PROCESSING 


The  following  describes  the  processing  performed  by  this  part: 


separate  (Wander_Azimuth  Navigation_Parts) 
package  body  Earth  Rotation  Rate  is" 


—  — local  declarations 
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NE  :  constant  Indices  :=  Indices' FIRST; 

NN  :  constant  Indices  :=  Indices' SUCC(NE) ; 

V  :  constant  Indices  :=  Indices' LAST; 


- unit  body- 


function  Compute  (DC_P_V  :  Direction_Cosine_Matrix_Elements; 

DC_P_NE  :  Direction_Cosine_Matrix_Elements; 
DC_P_NN  :  Direction_Cosine_Matrix_Elements) 
return  Angular_Velocity_Vectors  is 


— declaration  of  variables- 


Rotation_Rate  :  Angular_Velocity_Vectors; 


- beginning  of  function- 


begin 

Rotation_Rate(NE)  :=  DC_P_NE  *  Earth_Rate; 
RotationJRate(NN)  :=  DC_P_NN  *  Earth_Rate; 
Rotation_Rate(V)  :=  DC_P_V  *  Earth_Rate; 

return  Rotation_Rate; 

end  Compute; 

end  Earth_Rotation_Rate; 

3. 3. 2. 3. 9. 2. 7  UTILIZATION  OF  OTHER  ELEMENTS 
None. 


3. 3. 2. 3. 9. 2. 8  LIMITATIONS 
None. 


3. 3. 2. 3. 9. 2. 9  LLCSC  DESIGN 
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3.3.2.3.9.2.10  UNIT  DESIGN 
None. 


3.3.2.3.10  UNIT  DESIGN 

3.3.2.3.10.1  C0MPUTE_EAST_VEL0CITY  UNIT  DESIGN  (CATALOG  #P235-0) 


This  function  computes  the  east  velocity  given  the  nominal  east  and  north 
velocities  and  the  wander  angle. 


The  computations  are  performed  as  follows: 
VelE  :=  VelNE  *  Cos(VA)  -  VelNN  *  Sin(WA) 


where  VelNE  =  nominal  east  velocity 
VelNN  =  nominal  north  velocity 
WA  =  wander  angle 


3.3.2.3.10.1.1  REQUIREMENTS  ALLOCATION 
This  part  meets  CAMP  requirement  R158. 


3. .3. 2. 3. 10. 1.2  LOCAL  ENTITIES  DESIGN 
None. 


3.3.2.3.10.1.3  INPUT/OUTPUT 
GENERIC  PARAMETERS: 


The  following  generic  parameters  were  previously  defined  for  this  part's 
specification  in  the  Wander_Azimuth_Navigation_Parts  package  specification: 


Data  types: 

The  following  table  describes  the  generic  formal  types  required  by  this  part: 


Name 

Sin_Cos_Ratio 

Velocities 

Wander_Angles 


Type 


|  Description 


I 


floating 
point  type 
floating 
point  type 
floating 
point  type 


Data  type  of  results  from  a  sine  or  cosine 
operation 

Data  type  of  velocity  measurements 


Data  type  of  wander  angle  measurements 


Subprograms : 
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The  following  table  describes  the  generic  formal  subroutines  required  by  this 
part: 


|  Name 

Type 

Description 

1 

|  »*" 

function 

Multiplication  operator  defining 

the  operation:  | 

i 

Velocities  *  Sin  Cos  Ratio  =>  Velocities  1 

|  Sin  Cos 

procedure 

Returns  the  sine  and  cosine  of  a 

wander  angle  j 

FORMAL  PARAMETERS: 

The  following  table  describes  this  part's  formal  parameters: 


|  Name 

1  Type  | 

Mode  | 

Description  | 

1  Nominal  East 

|  Velocities  | 

In  | 

Velocity  which  would  be  east  if  the| 

|  Veloci ty 

1  1 

1 

wander  angle  was  0  | 

|  Nominal  North 

|  Velocities  | 

In  | 

Velocity  which  would  be  north  if  ! 

j  Velocity 

1  1 

1 

the  wander  angle  was  0  j 

j  Current 

|  Wander  Angles | 

In  j 

Current  wander  azimuth  angle  j 

|  Wander_Angle 

1 

1 

3.3.2.3.10.1.4  LOCAL  DATA 

• 

Data  objects: 

The  following  table  describes  the  data  objects  maintained  by  this  part: 

|  Name 

1  Type 

|  Value 

|  Description  | 

|  Cos  of  Angle 

1  Sin  Cos  Ratio 

|  N/A 

|  Cosine  of  current  wander  angle  j 

j  Sin_of_Angle 

j  Sin_Cos_Ratio 

|  N/A 

j  Sine  of  current_wander_angle  | 

3.3.2.3.10.1.5 

Not  applicable. 

3.3.2.3.10.1.6 

PROCESS  CONTROL 

PROCESSING 

The  following  describes  the  processing  performed  by  this  part: 


separate  (Wander_Azimuth  Navigation_Parts) 
function  Compute_East_VeIocity 

(NomInal_East_Velocity  :  Velocities; 

Nominal_NortK_Velocity  :  Velocities; 

Current  Wander  Angle  :  Wander  Angles)  return  Velocities  is 
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—  — declaration  of  variables- 


Cos_of_Angle  :  Sin_Cos_Ratio; 
Sin  of  Angle  :  Sin  Cos  Ratio; 


— begin  function  Compute  East  Velocity 


begin 


Sin_Cos(Current_Wander_Angle,  Sin_of_Angle,  Cos_of_Angle) ; 


return  (  Nominal_East_Velocity  *  Cos_of  Angle  - 
Nominal_North_Veiocity  *  3in_or_Angie) ; 


end  Corapute_East_Velocity; 


3.3.2.3.10.1.7  UTILIZATION  OF  OTHER  ELEMENTS 


None. 


3.3.2.3.10.1.8  LIMITATIONS 


None. 


3.3.2.3.10.2  COMPUTE_NORTH_VELOCITY  UNIT  DESIGN  (CATALOG  #P236-0) 


This  function  calculates  the  North  velocity  component  of  a  missile  given  the 
nominal  north  and  east  velocities  of  the  missile. 


The  computations  are  performed  as  follows: 


VelN  :=  VelNE  *  Sin(VA)  +  VelNN  *  Cos(WA) 


where  VelNE  =>  nominal  east  velocity 
VelNN  =  nominal  north  velocity 
WA  >  wander  angle 


i. 3. 2. 3. 10. 2.1  REQUIREMENTS  ALLOCATION 


This  part  meets  CAMP  requirement  R186. 


3.3.2.3.10.2.2  LOCAL  ENTITIES  DESIGN 


None. 


CAMP  Software  Detailed  Design  Document 


Page 


3.3.2.3.10.2.3  INPUT/OUTPUT 
GENERIC  PARAMETERS: 

The  following  generic  parameters  were  previously  defined  for  this  part's 
specification  in  the  Wander_Azimuth_Navigation_Parts  package  specification: 

Data  types: 

The  following  table  describes  the  generic  formal  types  required  by  this  part: 


|  Name  |  Type  |  Description 


Sin_Cos_Ratio 

Velocities 

Wander_Angle 


floating 
point  type 
floating 
point  type 
floating 
point  type 


Data  type  defining  results  of  a  sine  or 
cosine  operation 

Data  type  defining  velocity  measurements 
Data  type  defining  wander  angle  measurements 


Subprograms : 

The  following  table  describes  the  generic  formal  subroutines  required  by  this 
part: 


|  Name 

Type 

Description 

1 

|  '»*" 

function 

Multiplication  operator  defining  the  operation:  ! 

1 

Velocities  *  Sin  Cos  Ratio  => 

Velocities  j 

|  Sin  Cos 

procedure 

Procedure  calculating~sine  and 

cosine  values  of  a  j 

1 

wander  angle 

1 

FORMAL  PARAMETERS: 

The  following  table  describes  this  part's  formal  parameters: 


|  Name  |  Type  |  Mode  |  Description 


Nominal_East_ 

|  Velocities 

1 

In 

Velocity 

1 

l 

l 

Nominal_North_ 

1 

|  Velocities 

1 

In 

Velocity 

1 

1 

1 

1 

Current_ 

1 

|  Wander 

1 

1 

In 

Wander_Angle 

j  Angles 

1 

Velocity  in  the  direction  which 
would  be  East  if  the  wander  angle 
was  0 

Velocity  in  the  direction  which 
would  be. North  if  the  wander  angle 
was  0 

Value  of  the  current  wander  angle 
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3.3.2.3.10.2.4  LOCAL  DATA 
Data  objects: 

The  following  table  describes  the  data  objects  maintained  by  this  part: 


|  Name 

1  Type 

|  Value 

|  Description  | 

|  Cos  of  Angle 
j  Sin  of  Angle 

|  Sin  Cos  Ratio 
j  Sin_Cos  Ratio 

|  N/A 
j  N/A 

|  Cosine  of  current  wander  angle  j 
j  Sine  of  current_wander  angle  j 

3.3.2.3.10.2.5  PROCESS  CONTROL 
Not  applicable. 


3.3.2.3.10.2.6  PROCESSING 

The  following  describes  the  processing  performed  by  this  part: 

separate  (Wander_Azimuth_Navigation_Parts) 
function  Compute  North_Velocity 

(NominalJEast  Velocity  :  Velocities; 

Nominal_NortK_Velccity  :  Velocities; 

Current_Vander_Angle  :  Vander_Angles)  return  Velocities  is 


- declaration  of  variables- 


Cos_of_Angle  :  Sin_Cos_Ratio; 

Sin_of_Angle  :  Sin_Cos_Ratio; 

— begin  function  Compute_North_Veloeity 

begin 

Sin_Cos(Current_Wander_Angle,  Sin_of_Angle,  Cos_of_Angle) ; 

return  (  Nominal_East  Velocity  *  Sin_of  Angle  + 
Nominal_NortK_Velocity  *  Cos_oI_Angle) ; 

end  Compute_North_Velocity; 

3.3.2.3.10.2.7  UTILIZATION  OF  OTHER  ELEMENTS 
None. 


v;vv 


mm. 
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3.3.2.3.10.2.8  LIMITATIONS 
None. 

3.3.2.3.10.3  COMPUTE_EARTH_RELATIVE_HORIZONTAL_VELOCITIES  UNIT  DESIGN  (CATALOG 
#P237-0) 

This  procedure  computes  the  north  and  east  velocity  given  the  nominal  east  and 
north  velocities  and  the  wander  angle. 

The  computations  are  performed  as  follows: 

VelE  :=  VelNE  *  Cos(WA)  -  VelNN  *  Sin(WA) 

VelN  :=  VelNE  *  Sin(VA)  +  VelNN  *  Cos(WA) 

where  VelNE  =  nominal  east  velocity 
VelNN  =  nominal  north  velocity 
WA  =  wander  angle 

3.3.2.3.10.3.1  REQUIREMENTS  ALLOCATION 
This  part  meets  CAMP  requirement  R001. 

3.3.2.3.10.3.2  LOCAL  ENTITIES  DESIGN 
None. 

3.3.2.3.10.3.3  INPUT/OUTPUT 
GENERIC  PARAMETERS: 

The  following  generic  parameters  were  previously  defined  for  this  part's 
specification  in  the  Wander_Azimuth_Navigation_Parts  package  specification: 

Data  types: 

The  following  table  describes  the  generic  formal  types  required  by  this  part: 
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The  following  table  describes  the  generic  formal  subroutines  required  by  this 
part: 


|  Name 

1  Type 

|  Description  | 

|  »*" 

1  1 
|  Sin_Cos  | 

|  function 

|  procedure 

|  Multiplication  operator  defining  the  operation:  | 

j  Velocities  *  Sin_Cos_Ratio  =>  Velocities  | 

1  Returns  the  sine  and  cosine  of  an  input  angle  j 

FORMAL  PARAMETERS: 

The  following  table  describes  this  part's  formal  parameters: 


|  Name  |  Type  |  Mode  |  Description 


|  Nominal  East 
j  Velocity 

Velocities 

1  In 

1 

|  Velocity  in  the  direction  that 
j  would  be  East  if  the  wander 
|  angle  was  0 

j  Nominal  North 
j  Velocity 

Velocities 

1  In 

l 

j  Velocity  in  the  direct  that  would 
j  be  North  if  the  wander  angle  was 
n 

j  Current_ 

|  Wander  Angle 

Wander_Angles 

1  In 

1 

1 

j  Value  of  the  current  wander  angle 
! 

j  East_VeTocity 

Velocities 

j  Out 

1 

|  Current .east  velocity  of  the 
j  missile 

j  North_Velocity 

Velocities 

j  Out 

1 

j  Current  north  velocity  of  the 
j  missile 

3.3.2.3.10.3.4  LOCAL  DATA 
Data  objects: 

The  following  table  describes  the  data  objects  maintained  by  this  part: 


|  Name 

Type 

|  Value 

|  Description  | 

|  Cos_of_Angle 
j  Sin_of_Angle 

Sin_Cos_Ratio 

Sin_Cos_Ratio 

|  N/A 
j  N/A 

|  Cosine  of  current_wander_angle  | 

j  Sine  of  current_wander_angle  | 

3.3.2.3.10.3.5  PROCESS  CONTROL 
Not  applicable. 


3.3.2.3.10.3.6  PROCESSING 

The  following  describes  the  processing  performed  by  this  part: 
separate  (Wander_Azimuth_Navigation_Parts) 


*i*J.\s*  J-* 
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procedure  Compute_Earth_Relative_Horizontal_Velocities 
(Nominal_East_Velocity  :  in  Velocities; 

Nominal_North_Velocity  :  in  Velocities; 

Current_Wander_Angle  :  in  Wander_Angles ; 

East_Velocity  :  out  Velocities; 

North  Velocity  :  out  Velocities)  is 


- declaration  of  variables- 


Cos_of_Angle  :  Sin_Cos_Ratio; 
Sin  of  Angle  :  Sin~Cos  Ratio; 


— begin  procedure  Compute_Earth_Relative_Horizontal_Velocities 


begin 


Sin_Cos(Current_Wander_Angle,  Sin_of_Angle,  Cos_of_Angle) ; 


East_Velocity  :=  Nominal_East  Velocity  *  Cos_of  Angle  - 
Nominal  NortK_Velocity  *  Sin_oI_Angle; 
North_Velocity  :=*  NominaI_East  Velocity  *  Sin~of  Angle  + 
Nominal_NortH_Velocity  *  Cos_oI_Angle; 


end  Compute  Earth  Relative  Horizontal  Velocities; 


3.3.2.3.10.3.7  UTILIZATION  OF  OTHER  ELEMENTS 


None. 


3.3.2.3.10.3.8  LIMITATIONS 


None. 


Page  154 


3.3.2.3.10.4  C0MPUTE_T0TAL_ANGULAR_VEL0CITY  UNIT  DESIGN  (CATALOG  #P238-0) 


This  function  computes  the  total  angular  velocity  (which  represents  the  ground 
speed)  given  the  nominal  north  and  east  nominal  velocities. 


The  computations  are  performed  as  follows: 


TAVel  :=  Sqrt(  RhoNN**2  +  RhoNE**2) 


where  RhoNN  =  nominal  north  component  of  the  current  rotation  rate  of  the 
missile's  navigation  frame  with  respect  to  the  earth  RhoNE  =  nominal  east 
component  of  the  current  rotation  rate  of  the  missile's  navigation  frame  with 
respect  to  the  earth 
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3.3.2.3.10.4.1  REQUIREMENTS  ALLOCATION 
This  part  meets  CAMP  requirement  R004. 


3.3.2.3.10.4.2  LOCAL  ENTITIES  DESIGN 
None. 


3.3.2.3.10.4.3  INPUT/OUTPUT 
GENERIC  PARAMETERS; 

The  following  generic  parameters  were  previously  defined  for  this  part's 
specification  in  the  Vander_Azimuth_Navigation_Parts  package  specification: 

Data  types: 

The  following  table  describes  the  generic  formal  types  required  by  this  part: 


-ii 


|  Name  | 

Type  | 

Description  | 

1  Angular  Velocities  | 

j  Angular  Velocity  Squared  | 

1  1 

floating  ] 
point  type  j 
floating  j 
point  type  | 

1 

Data  type  used  for  angular 
velocity  measurements 

Data  type  of  object  resulting 
from  the  multiplication  of  two 
objects  of  type  Angular_Velocity 

Subprograms: 


88 


The  following  table  describes  the  generic  formal  subroutines  required  by  this 
part: 


|  Name  |  Type  |  Description 


»*" 


I  Sqrt 


|  function  |  Multiplication  operator  defining  the  operation: 
|  ]  Angular_Velocities  *  Angular_Velocities  => 

|  j  Angular~Velocity  Squared 

|  function  |  Square  root  function 


FORMAL  PARAMETERS: 

The  following  table  describes  this  part's  formal  parameters: 
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|  Name  |  Type  |  Mode  |  Description 


Rho_Eas  t 

1 

Angular_ 

1  In 

i 

| 

Velocities 

Rho  North 

1 

Angular_ 

1  In 

1 

Velocities 

1 

1 

Nominal  east  component  of  the  current 
rotation  rate  of  the  missile's  naviga¬ 
tion  frame  with  repsect  to  the  Earth 
Nominal  north  component  of  the  current 
rotation  rate  of  the  missile's  naviga¬ 
tion  frame  with  repsect  to  the  Earth 


3.3.2.3.10.4.4  LOCAL  DATA 
None. 


3.3.2.3.10.4.5  PROCESS  CONTROL 
Not  applicable. 


3.3.2.3.10.4.6  PROCESSING 

The  following  describes  the  processing  performed  by  this  part: 

separate  (Wander_Azimuth_Navigation_Parts) 
function  Compute_Total_Angular_Velocity 

(RhoEast  AngularVelocities; 

Rho_North  :  AngularVelocities)  return  Angular_Velocities  is 


begin 

return  Sqrt(Rho_East  *  Rho_East  +  Rho_North  *  Rho_North); 
end  Compute_Total_Angular_Velocity; 

3.3.2.3.10.4.7  UTILIZATION  OF  OTHER  ELEMENTS 


None. 


3.3.2.3.10.4.8  LIMITATIONS 


None. 


3.3.2.3.10.5  COMPUTE_CORIOLIS_ACCELERATION  UNIT  DESIGN  (CATALOG  #P239-0) 

This  function  computes  the  Coriolis  acceleration  given  the  velocity  vector,  Rho 
vector,  and  Omega  vector. 

The  computations  are  performed  as  follows: 

OorAccei  »-  (?.  *  Omega  Vector  +  Rho  Vectot)  x  Velocity  Vector 


VVT Or'Or*.v  \  • 


V.N' 


■w  -  V  V  V  V"V  v  V*. 
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3.3.2.3.10.5.1  REQUIREMENTS  ALLOCATION 
This  part  meets  CAMP  requirement  R007. 


3.3.2.3.10.5.2  LOCAL  ENTITIES  DESIGN 


None. 


3.3.2.3.10.5.3  INPUT/OUTPUT 
GENERIC  PARAMETERS: 

The  following  generic  parameters  were  previously  defined  for  this  part's 
specification  in  the  Wander_Azimuth_Navigation_Parts  package  specification: 

Data  types: 

The  following  table  describes  the  generic  formal  types  required  by  this  part: 


|  Name 

Type 

|  Acceleration_Vector 

private 

! 

|  Angular_Velocity_Vector 

private 

1 

|  Velocity_Vector 

1 

private 

Subprograms: 

Des.cription 


Contains  East,  North,  and  vertical 
components  of  the  Coriolis 
acceleration 

Contains  East,  North,  and  vertical 
components  of  the  Earth's 
rotation  rate 
Contains  East, 
components  of 
velocity 


North,  and  vertical 
the  missile's 


The  following  table  describes  the  generic  formal  subroutines  required  by  this 
part: 


j  Name 


Type  |  Description 


Sparse_Righ  t_X_Add 
Sparse_Right_Z_Add 
Cross  Product 


function 

function 

function 


Adds  two  Angular_Velocity_Vector's 
assuming  the  x-component-of  the  second 
vector  equals  0 

Adds  two  Angular_Velocity_Vec tor's 
assuming  the  z-component-of  the  second 
vector  equals  0 

Cross  product  function  crossing  an 
Angular_Velocity_Vector  with  a 
Velocity_Vector  to  obtain  an 
Acceleration  Vector 
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FORMAL  PARAMETERS: 

The  following  table  describes  this  part's  formal  parameters: 


j  Name  |  Type  |  Mode  |  Description 


Velocity_In 


Veloci ty_Vector 


In 


Rho  in 


Angular_ 

Veloci ty_Vec tor 


In 


Omega_in 


Angular_ 

Veloci ty_Vector 


In 


Contains  the  nominal  East, 

North,  and  vertical  components 
of  the  missile's  current 
velocity 

Contains  the  nominal  East,  North, 
and  vertical  components  of  the 
current  earth-relative 
navigation  frame  rotation  rate 
(vertical  component  assumed  to 
equal  0  and  is  therefore 
ignored) 

Contains  the  nominal  East,  North, 
and  vertical  components  of  the 
current  Earth's  rotation  rate 


3.3.2.3.10.5.4  LOCAL  DATA 


Data  objects:. 

The  following  table  describes  the  data  objects  maintained  by  this  part: 


|  Name 

I  Type 

|  Value 

|  Description  | 

|  Temp  A  V  V 

1 

1 

|  Angular_ 

[  Veloci ty_ 

|  Vector 

|  N/A 

1 

1 

|  Temporary  angular  velocity  | 

|  vector  | 

1  1 

3.3.2.3.10.5.5 

Not  applicable. 

PROCESS  CONTROL 

3.3.2.3.10.5.6 

PROCESSING 

The  following  describes  the  processing  performed  by  this  part: 


separate  (Wander_Azimuth_Navigation_Parts) 
function  Compute_Coriolis_Acceleration 

(Veloci ty_In  :  Veloci ty_Vec tors; 

Rho_In  :  Angular_Velocity_Vectors; 

0mega_In  :  Angular_Velocity_Vectors) 
return  Acceleration  Vectors  is 


-declaration  of  variables- 


L.**  <i 


,N  ■'V  Jj%  JV  m  f 
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Temp_A_V_V  :  Angular_VeIocity_Vectors; 

— begin  function  Compute_Coriolis_Acceleration 
begin 

Temp_A_V_V  :=  Omega_In  +  Omega_In; 

Temp_A_V_V  :=  Spars e_Right_Z_Add(Temp_A_V_V,  Rho_In); 

return  Cross_Product(Temp_A_V_V,  Velocity_In); 

end  Compute_Coriolis_Acceleration; 

3.3.2.3.10.5.7  UTILIZATION  OF  OTHER  ELEMENTS 
None. 

3.3.2.3.10.5.8  LIMITATIONS 
None. 


3.3.2.3.10.6  COMPUTE_CURVATURES  UNIT  DESIGN  (CATALOG  #P241-0) 

This  generic  function  computes  the  inverse  radius  of  curvature  vector  given 
three  elements  of  the  direction  cosine  matrix  and  the  current  altitude. 

3.3.2.3.10.6.1  REQUIREMENTS  ALLOCATION 
This  part  meets  CAMP  requirement  R035. 


3.3.2.3.10.6.2  LOCAL  ENTITIES  DESIGN 
None. 


3.3.2.3.10.6.3  INPUT/OUTPUT 
GENERIC  PARAMETERS: 

The  following  generic  parameters  were  previously  defined  in  the  package 
specification  of  Wander_Azimuth_Navigation_Parts: 

Data  types: 

The  following  table  summarizes  the  generic  formal  types  required  ry  this  part: 
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|  Name 
Indices 

Direction_Cosine_ 

Matrix_Elements 

Distances 

Inverse_Distances 

Real 

Inverse_Dis  tance_ 
Vectors 


|  Description 

Used  to  dimension  Distance_Vectors 

Used  to  define  elements  of  a 
direction  cosine  matrix 

Data  type  defining  distance 
measurements 

Data  type  of  one  over  distance 
measurements 

Data  type  used  to  define  Earth_ 
Flat  tening_Coef f icient 

Array  of  ,,Inverse_Distances" 
dimensioned  by  "Indices" 


discrete 

type 

floating 
point  type 
floating 
point  type 
floating 
point  type 
floating 
point  type 
array 


Data  objects: 

The  following  table  summarizes  the  generic  formal  objects  required  by  this 
part: 


I  Type 


Description 


|  Earth  Flattening 
j  Coefficient 

1 

1 

Real 

Coefficient  of  earth  flattening 

j  0ne_0ver_Earth_Radius 

1 

1 

Inverse_ 

Distances 

Value  of  1/Earth  radius 

|  NE 

1 

Indices 

Indices 'FIRST 

j  NN 

1 

Indices 

Indices' SUCC(NE) 

j  V 

I 

Indices 

Indices' LAST 

j  Two_Flat 

1 

Real 

2.0  *  Earth_Flattening_Coeff icient 

Subprograms: 

The  following  table  summarizes  the  generic  formal  subroutines  required  by  this 
part: 


|  Name  |  Type  |  Description 


function 

function 

function 


Multiplication  operator  defining  the  operation: 
Distances  *  Inverse_Distances  =»>  Real 
Multiplication  operator  defining  the  operation: 
Real  *  Direction_Cosine_Matrix_Elements  =>  Real 
Division  operator~defining  the  operation: 

Inverse  Distances  *  Real  =>  Inverse  Distances 


FORMAL  PARAMETERS: 

The  following  table  describes  this  part's  formal  parameters: 


fM 


mwM 
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A. 

> 

|  Name 

1  Type 

|  Mode 

|  Description 

I 

: 

|  DC  P  V 

1  Direction  Cosine 

1  In 

|  Sine  of  the  current  latitude  of 

1 

' 

j  Matrix  Elements 

1 

j  the  missile 

\ 

|  DC  P  NE 

Direction  Cosine 

1  In 

Product  of  the  sine  of  the  current 

j  Matrix  Elements 

1 

j  wander  angle  and  the  cosine  of 

1 

\ 

1 

1 

|  the  latitude 

1 

|  DC  P  NN 

|  Direction  Cosine 

j  In 

j  Product  of  the  cosine  of  the 

i 

i  Matrix  Elements 

1 

j  wander  angle  and  the  cosine  of 

' 

1 

j  the  latitude 

|  Current 

j  Distances 

1  In 

j  Current  altitude  of  the  missile 

|  Altitude 

1 

1 

1 

1 

l 

3.3.2.3.10.6 

.4  LOCAL  DATA 

- 

i 

Data  objects 

5 

i 

The  following  table  describes  the  data  objects  maintained  by  this  part: 

5 

-> 

E  € 


->'v 


'-m 


VS? 


|  Name 

1 

Type 

1 

Description 

|  Denominator 

T 

Real 

1 

Intermediate  calculation 

j  Distance_Ratio 

i 

Real 

Intermediate  calculation 

j  Partial_Denominator 

i 

Real 

Intermediate  calculation 

j  Curvatures 

i 

Inverse_Distance_ 

Vector  being  calculated  and 

i 

Vectors 

1 

returned 

|  Two_Flat_x_DC_P_NN 

i 

Real 

1 

Intermediate  calculation 

3.3.2.3.10.6.5  PROCESS  CONTROL 
Not  applicable. 


3.3.2.3.10.6.6  PROCESSING 

The  following  describes  the  processing  performed  by  this  part: 


separate  (Wander_Azimuth_Navigation_Parts) 
function  Compute_Curvatures 

(DC_P _V  :  Direction_Cosine_Matrix_Elements; 

DC_P_NE  :  Direction_Cosine_Matrix_Elements; 

DC_P_NN  :  Direction_Cosine_Matrix_Elements; 

Current_Altitude  :  Distances) 

return  Inverse  Distance  Vectors  is 


- declaration  of  variables- 


Denominator 
Distance  Ratio 


:  Real; 
:  Real; 
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Partial_Denominator  s  Real; 

Curvatures  :  Inverse_Distance_Vectors; 

Two  Flat  x  DC  P  NN  :  Real; 


— beginning  of  function- 


begin 

Distance_Ratio  :=  Current_Altitude  *  One_Over_Earth_Radius; 

Partial_Denominator  :=  1.0  - 

Distance_Ratio  - 
Earth_Flattening_Coefficient  * 

(DC_P_V  *  DC_P_V) ; 

Denominator  :=  Partial_Denominator  + 

Two_Flat  *  (DC_P_NE  *  DC_P_NE); 

Curvatures(NE)  :=  0ne_0ver_Earth__Radius  *  Denominator; 

Two_Flat_x_DC_P_NN  :=  Two_Flat  *  DC_P_NN; 

Denominator  :=  Partial  Denominator  + 

Two_Flat_x_DC_P  NN  *  DC_P_NN; 

Curvatures (NN)  s*  One_Over_Earth_Racfius  *  Denominator; 

Denominator  :=*  Two_Flat_x_DC  P_NN  *  DC_P_NE; 

Curvatures (V)  :  =  .0ne_0ver_EartK_Radius  *  Denominator; 

return  Curvatures; 

end  Compute_Curvatures; 


3.3.2.3.10.6.7  UTILIZATION  OF  OTHER  ELEMENTS 
None. 


3.3.2.3.10.6.8  LIMITATIONS 
None. 


3.3.2.3.10.7  T0TAL_PLATF0RM_R0TATI0N_RATE  (FUNCTION  BODY)  UNIT  DESIGN  (CATALOG 
#P242-0) 

This  function  computes  the  rotation  rate  of  the  navigation  coordinate  system 
with  respect  to  inertial  space  given  the  Rho  and  Omega  vectors. 

The  computations  performed  are  as  follows: 

TPR  :=  Rho  Vector  +  Omega  Vector 
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3.3.2.3.10.7.1  REQUIREMENTS  ALLOCATION 
See  main  header  for  enclosing  package. 

3.3.2.3.10.7.2  LOCAL  ENTITIES  DESIGN 
None. 

3.3.2.3.10.7.3  INPUT/OUTPUT 
GENERIC  PARAMETERS: 

The  following  generic  parameters  were  previously  defined  when  this  part  was 
specified. 

Data  types: 

The  following  table  describes  the  generic  formal  types  required  by  this  part: 


|  Name  | 

Type 

|  Description  | 

|  Angular_Velocity_Vectors| 

private 

|  Array  of  angular  velocity  measurements  | 

Subprograms: 

The  following  table  describes  the  generic  formal  subroutines  required  by  this 
part: 


|  Name 

1  Type 

|  Description 

1 

|  Sparse  Right  Z 
|  Add 

1 

|  function 

i  i 

i 

|  Adds  two  angular  velocity  vectors 
[  the  third  component  of  the  right 
|  equals  0 

assuming  | 
array  1 

1 

FORMAL  PARAMETERS: 

The  following  table  describes  this  part's  formal  parameters: 
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|  Name 


I  Type 


j  Mode  |  Description 


Rho  Vector 


Omeea  Vector 


Angular_ 

Veloci ty_Vectors 


Angular_ 

Velocity  Vectors 


Contains  the  nominal  East, 
North,  and  vertical  components 
of  the  rotation  rate  of  the 
navigation  coordinate  system 
with  respect  to  the  earth 
(vertical  component  assumed  to 
equal  0  and  is  therefore 
ignored) 

Contains  the  nominal  East, 
North,  and  vertical  components 
of  the  earth's  rotation  rate 


3.3.2.3.10.7.4  LOCAL  DATA 


None. 


3.3.2.3.10.7.5  PROCESS  CONTROL 


Not  applicable. 


3.3.2.3.10.7.6  PROCESSING 


The  following  describes  the  processing  performed  by  this  part: 


separate  (Wander_Azimuth_Navigation_Parts) 
function  Total_Platform_Rotation  Rate 

(Rho_Vector  :  AnguTar_Velocity_Vectors; 
Omega_Vector  :  Angular_Velocity_Vectors) 
return  Angular  Veloci ty_Vectors  is 

begin 

return  Sparse_Right_Z_Add(Left  =>  0mega_Vector, 

Right  =»>  Rho_Vector)$ 
end  Total_Platform_Rotation_Rate; 


3.3.2.3.10.7.7  UTILIZATION  OF  OTHER  ELEMENTS 


None. 


3.3.2.3.10.7.8  LIMITATIONS 


None. 


3.3.2.3.10.8  C0MPUTE_EARTH  RELATIVE JJAVIGATI0NJR0TATI0N_RATE  UNIT  DESIGN  (CATALOG 
#P244-0) 


This  function  computes  the  rotation  rate  of  the  navigation  coordinate  system 
with  respect  to  the  Earth  given  the  Velocity  and  Radius  of  Curvature  vectors. 
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The  computations  are  performed  as  follows: 

RhoNE  :=  -  (VelNE  *  RCV)  -  (VelNN  *  RCNN) 

RhoNN  :=  (VelNE  *  RCNE)  +  (VelNN  *  RCV  ) 

where  VelNx  =  nominal  x-component  of  the  missile's  current  velocity 

RCx  =  nominal  x-component  of  the  radius  of  curvature 

3.3.2.3.10.8.1  REQUIREMENTS  ALLOCATION 
This  part  meets  CAMP  requirement  R025. 

3.3.2.3.10.8.2  LOCAL  ENTITIES  DESIGN 
None. 


3.3.2.3.10.8.3  INPUT/OUTPUT 
GENERIC  PARAMETERS: 


The  following  generic  parameters  were  previously  defined  in  the  package 
specification  of  Wander_Azimuth_Navigation_Parts: 

Data  types: 

The  following  table  summarizes  the  generic  formal  types  required  by  this  part: 


|  Name 


Type 


|  Description 


Velocities 
Inverse_Distances 
Angular_Veloci ties 
Indices 

Angular_Velocity_Vectors 
Veloci ty_Vectors 
Inverse  Distance  Vectors 


floating 
point  type 
floating 
point  typd 
floating 
point  type 
discrete 
type 
array 

array 

array 


Data  type  defining  velocity 
measurements 

Data  type  defining  inverse 
distance  measurements 
Data  type  defining  angular 
velocity  measurements 
Data  type  used  to  dimension 
vector  types 

Array  of  angular  velocities 
dimensioned  by  "Indices” 

Array  of  velocities  dimensioned 
by  "Indices” 

Array  of  inverse  distances 
dimensioned  by  "Indices" 


Subprograms: 

The  following  table  summarizes  the  generic  formal  subroutines  required  by  this 
part: 


J*_A  jLJl  ^ 
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|  Name 

i  Type  | 

|  Description  | 

|  "/" 

1 

|  function 

1 

|  Division  operator  defining  the  operation:  | 

!  Velocities  *  Inverse  Distances  =>  Angular_Velocities  j 

FORMAL  PARAMETERS: 

The  following  table  describes  the  formal  parameters  to  the  unit  in  this  part: 


|  Name  | 

Type  | 

Mode 

|  Description  | 

|  Missile_  | 
j  Velocity  j 

1  1 

Velocity_Vectors | 

In 

|  Contains  nominal  East  and  North  { 

|  components  of  the  missile's  j 

j  current  velocity  ! 

j  Curvatures  | 

1  1 

1  1 

Inverse  j 

Distance  j 

Vectors  j 

In 

j  Contains  nominal  East,  North,  and  j 
j  vertical  components  of  the  j 
j  inverse  radius  of  curvature  j 

3.3.2.3.10.8.4 

LOCAL  DATA 

Data  objects: 

The  following  table  describes  the  data  objects  maintained  by  this  part: 


|  Name 

|  Type  |  Value 

|  Description  | 

|  Rho 

1 

|  Angular_  |  N/A 
j  Velocity_  | 
j  Vectors  1 

|  Vector  being  calculated  and  j 

j  returned  | 

1  1 

3.3.2.3.10.8.5  PROCESS  CONTROL 


Not  applicable. 


3.3.2.3.10.8.6  PROCESSING 

The  following  describes  the  processing  performed  by  this  part: 

separate  (Vander_Azimuth_Navigation  Parts) 
function  Compute_Earth_Relative_NavIgation_Rotation_Rate 
(Missile_Velocity  :  Velocity_Vectors; 

Curvatures  :  Inverse_Distance_Vectors) 

return  Angular_Velocity_Vectors  is 


—  — declaration  of  variables- 


'  <V  A  A  A 
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Rho  :  Angular_Velocity  Vectors; 


— beginning  of  function- 


5 

i 

A 


& 


begin  | 

Rho(NE)  :=  -  Missile_Velocity(NE)  *  Curvatures(V)  -  ; 

Missile_Velocity(NN)  *  Curvatures (NN) ;  | 

Rho(NN)  :=  Missile_Velocity(NE)  *  Curvatures (NE)  +  \ 

Missile_Velocity(NN)  *  Curvatures(V) ;  \ 

Rho(V)  :=  0.0;  | 

A 

% 

return  Rho;  1 

£ 

end  Compute_Earth_Relative_Navigation_Rotation_Rate;  | 


3.3.2.3.10.8.7  UTILIZATION  OF  OTHER  ELEMENTS 
None. 

3.3.2.3.10.8.8  LIMITATIONS 


None. 


3.3.2.3.10.9  C0MPUTE_LATITUDE  UNIT  DESIGN  (CATALOG  #P245-0) 

This  function  computes  the  current  latitude  given  the  sine  of  the  latitude. 


3.3.2.3.10.9.1  REQUIREMENTS  ALLOCATION 
See  main  header  for  enclosing  package. 


3.3.2.3.10.9.2  LOCAL  ENTITIES  DESIGN 


None. 

3.3.2.3.10.9.3  INPUT/OUTPUT 
GENERIC  PARAMETERS: 

The  following  generic  parameters  were  previously  defined  when  this  part  was 
specified: 


Data  types: 

The  following  table  describes  the  generic  formal  types  required  by  this  part: 


4 


ns 
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j  Name  |  Type  |  Description 


|  Earth_Positions|  floating  |  Data  type  used  to  define  longitude  and  j 
]  j  point  type  j  latitude  measurements  j 
j  Sin_Cos_Ratio  j  floating  j  Data  type  used  to  define  results  from  a  j 
j  j  point  type  j  sine  or  cosine  operation  j 


Subprograms : 

The  following  table  describes  the  generic  formal  subroutines  required  by  this 
part: 


|  Name 

Type 

Description  | 

|  Arcsin 

function 

Arcsine  function  | 

FORMAL  PARAMETERS: 

The  following  table  describes  the  formal  parameters  for  this  part: 


|  Name  |  Type  |  Mode  |  Description 

Sin_Latitude  Sin_Cos_Ratio  |  In  j  Sine  of  the  missile's  current 
“|j  latitude 


3.3.2.3.10.9.4  LOCAL  DATA 
None. 


3.3.2.3.10.9.5  PROCESS  CONTROL 
Not  applicable. 


3.3.2.3.10.9.6  PROCESSING 

The  following  describes  the  processing  performed  by  this  part: 

separate  (Wander_Azimuth_Navigation_Parts) 

function  Compute_Latitude  (Sin_Latitude  :  Sin_Cos  Ratio) 

return  EarthJPositions  Ts 

begin 

return  Arcsin(Sin_Latitude) ; 
end  Compute_Latitude7 
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3.3.2.3.10.9.7  UTILIZATION  OF  OTHER  ELEMENTS 


None. 


3.3.2.3.10.9.8  LIMITATIONS 


None. 


3.3.2.3.10.10  COMPUTE_LATITUDE_USING_ARCTANGENT  UNIT  DESIGN  (CATALOG  #P246-0) 

This  function  computes  the  current  Latitude  given  some  elements  of  the 
Direction  Cosine  matrix. 

The  computations  are  performed  as  follows: 

Lat  :  =  Arctan(DCPV  /  SqRT(DCPNE**2  +  DCPNN**2) 

(see  formal  parameters  section  for  explanation  of  abbreviations) 


3.3.2.3.10.10.1  REQUIREMENTS  ALLOCATION 
This  part  meets  CAMP  requirement  R030 


3.3.2.3.10.10.2  LOCAL  ENTITIES  DESIGN 
None. 


3.3.2.3.10.10.3  INPUT/OUTPUT 
GENERIC  PARAMETERS: 

The  following  generic  parameters  were  previously  defined  for  this  part's 
specification  in  the  Wander_Azimuth_Navigation_Parts  package  specification: 

Data  types: 

The  following  table  describes  the  generic  formal  types  required  by  this  part: 


|  Name  |  Type  |  Description  j 


|  Earth  Position 

|  floating 

1 

Data  type  used  to  define  latitude  and 

|  point  type 

1 

longitude  measurements 

|  Tan  Ratio 

|  floating 

1 

D-.>,ta  type  used  to  define  results  of  a 

j  point  type 

tangent  operation 

Direction  Cosine 

j  floating 

Data  type  used  to  define  elements  of  a 

|  Matrix_Elements 

j  point  type 

1 

direction  cosine  matrix 

Subprograms: 


CAMP  Software  Detailed  Design  Document 


The  following  table  describes  the  generic  formal  subroutines  required  by  this 
part: 


j  Name  |  Type  |  Description 


n  yii 


Arc tan 
Sqrt 


function 


function 

function 


Division  operator  defining  the  operation: 
Direction_Cosine_Matrix_Elements  / 
Direction_Cosine_Matrix_Elements  =>  Tan_Ratio 
Arctangent  function 
Square  root  function 


FORMAL  PARAMETERS: 

The  following  table  describes  this  part's  formal  parameters: 


|  Name  |  Type  |  Mode  J  Description 


DC_P_V  j  Direction  Cosine_  |  In 

j  MatrixJElements  j 

DC_P_NE  j  Direction  Cosine  j  In 

j  MatrixJBlements-  j 


DC_P_NN  |  Direction  Cosine 
j  Matrix_Elements’ 


In 


Sine  of  the  current  latitude  of  the 
missile 

Product  of  the  sine  of  the  wander 
angle  and  the  cosine  of  the 
latitude 

Product  of  the  cosine  of  the  wander 
angle  and  the  cosine  of  the 
latitude 


3.3.2.3.10.10.4  LOCAL  DATA 


Data  objects: 

The  following  table  describes  the  data  objects  maintained  by  this  part: 


|  Name 

! 

Type 

Value 

Description  | 

Denominator 

! 

Direction  Cosine 

N/A 

Temporary  variable  used  to  | 

1 

Matrix  Elements 

hold  results  of  intermediate  j 

1 

calculations  j 

3.3.2.3.10.10.5  PROCESS  CONTROL 
Not  applicable. 

3.3.2.3.10.10.6  PROCESSING 


The  following  describes  the  processing  performed  by  this  part: 

separate  (Wander_Azimuth_Navigation_Parts) 
function  Compu t e  La t i tude  Using_Ar cTangen t 
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(DC_P_V  :  Direction_Cosine_Matrix_Elements; 
DC_P_NE  :  Direction_Cosine_Matrix_Elements; 
DC_P_NN  :  Direction_Cosine_Matrix_Elements) 
return  Earth  Positions  is 


- declaration  of  variables- 


Denominator  :  Direction_Cosine_Matrix_Elements; 
— beginning  of  function- 
begin 

Denominator  :=  Sqrt(  DC_P_NE  *  DC_P_NE  + 

DC_P_NN  *  DC_P_NN  ); 

return  Arctan(DC_P_V  /  Denominator); 

end  ComputeJLati tude_Using_Arctangent ; 

3.3.2.3.10.10.7  UTILIZATION  OF  OTHER  ELEMENTS 
None. 


3.3.2.3.10.10.8  LIMITATIONS 
None. 


3.3.2.3.10.11  C0MPUTE_L0NGITUDE  UNIT  DESIGN  (CATALOG  #P247-0) 

This  function  computes  the  current  longitude  given  some  elements  of  the 
direction  cosine  matrix. 


3.3.2.3.10.11.1  REQUIREMENTS  ALLOCATION 
This  part  meets  CAMP  requirement  R033 


3.3.2.3.10.11.2  LOCAL  ENTITIES  DESIGN 
None. 


3.3.2.3.10.11.3  INPUT/OUTPUT 


GENERIC  PARAMETERS: 
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The  following  generic  parameters  were  previously  defined  for  this  part's 
specification  in  the  Wander_Azimuth_Navigation_Parts  package  specification: 

Data  types: 

The  following  table  describes  the  generic  formal  types  required  by  this  part: 


|  Name 

1  Type  | 

Description  | 

|  Earth_Position 

|  Tan  Ratio 

j  Direction_Cosine 
|  Matrix_Elements 

|  floating  j 

j  point  type  | 
j  floating  j 

j  point  type  | 

|  floating  j 

]  point  type  j 

Data  type  used  to  define  latitude  and  | 

longitude  measurements  j 

Data  type  used  to  define  results  of  a  j 

tangent  operation  j 

Data  type  used  to  define  elements  of  a  j 
direction  cosine  matrix  j 

Subprograms: 

The  following  table  describes  the  generic  formal  subroutines  required  by  this 
part: 

|  Name  |  Type 

|  Description 

1 

"/"  |  function 

Arctan  |  function 

j  Division  operator  defining  the  operation: 
j  Direction_Cosine_Matrix_Elements  / 
j  Direction_Cosine_Matrix_Elements  =>  Tan_Ratio 
|  Arctangent~function  ~  " 

FORMAL  PARAMETERS: 

The  following  table 

describes  this 

part's  formal  parameters: 

|  Name  |  Type 

|  Mode  |  Description  | 

|  DC  R_V  |  Direction  Cosine_  |  In 

|  |  Matrix_Eleraents  j 

1  °C_G_V  |  Direction  Cosine_  |  In 

|  j  Matrix_Elements  j 

|  Product  of  the  cosine  of  the  current  | 
j  latitude  and  the  sine  of  the  j 
|  current  longitude  j 
j  Product  of  the  cosine  of  the  current  j 
j  latitude  and  the  cosine  of  the  j 
j  current  longitude  j 

3.3.2.3.10.11.4  LOCAL  DATA 


None. 
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3.3.2.3.10.11.5  PROCESS  CONTROL 
Not  applicable. 

3.3.2.3.10.11.6  PROCESSING 

The  following  describes  the  processing  performed  by  this  part: 
separate  (Wander_Azimuth_Navigation_Parts) 

function  Compute_Longitude  (DC_R_V  :  Direction_Cosine_Matrix_Elements; 

DCjG_V  :  Direction_Cosine_Matrix_Elements) 
return  Ear th_Posit ions  is 

begin 

return  Arctan(DC_R_V  /  DC_G_V); 
end  Compute_Longitude; 

3.3.2.3.10.11.7  UTILIZATION  OP  OTHER  ELEMENTS 
None. 

3.3.2.3.10.11.8  LIMITATIONS 
None. 

3.3.2.3.10.12  COMPUTE J?ANDER_AZIMUTH_ANGLE  UNIT  DESIGN  (CATALOG  #P248-0) 

This  function  computes  the  current  wander  azimuth  angle  given  some  elements  of 
the  Direction  Cosine  matrix. 

The  computations  are  performed  as  follows: 

WA  :=  Arctan(DCPNE  /  DCPNN) 

(see  formal  parameters  section  for  explanation  of  abbreviations) 

3.3.2.3.10.12.1  REQUIREMENTS  ALLOCATION 
This  part  meets  CAMP  requirement  R02S 

3.3.2.3.10.12.2  LOCAL  ENTITIES  DESIGN 


None. 
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3.3.2.3.10.12.3  INPUT/CUTPUT 
GENERIC  PARAMETERS: 

The  following  generic  parameters  were  previously  defined  for  this  part's 
specification  in  the  Wander_Azimuth_Navigation_Parts  package  specification: 

Data  types: 

The  following  table  describes  the  generic  formal  types  required  by  this  part: 


Name 

Tan_Ratio 

Direction  Cosine_ 
Mat  r ix_Elemen t  s” 
Wander  Angles 


Type 


Description 


|  floating 

1 

Data  type  defining  results  of  a  tangent 

j  point  type 

1 

function 

|  floating 

1 

Data  type  defining  elements  in  a 

j  point  type 

1 

direction  cosine  matrix 

|  floating 

1 

Data  type  defining  wander  angle 

!  point  type 

1 

measurements 

Subprograms : 

The  following  table  describes  the  generic  formal  subroutines  required  by  this 
part: 


|  Name 

Type 

|  Description  | 

|  "/" 

function 

|  Devision  operator  defining  the  operation: 

1 

\  Direction  Cosine  Matrix  Elements  / 

i 

[  Direction  Cosine  Matrix  Elements  =>  Tan  Ratio 

|  Arctan 

function 

j  Arctangent  function 

FORMAL  PARAMETERS: 

The  following  table  describes  this  part's  formal  parameters: 


|  Name 

Type  |  Mode 

Description  | 

DC  P  NE 

Direction  Cosine  |  In 

Product  of  the  sine  of  the  wander  | 

Matrix Elements  j 

angle  and  the  cosine  of  the  j 

latitude  j 

DC  P  NN 

Direction  Cosine  j  In 

Product  of  the  cosine  of  the  wander  j 

Matrix_ETements  j 

angle  and  the  cosine  of  the  j 

latitude  j 

3.3.2.3.10.12.4  LOCAL  DATA 


None. 
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3.3.2.3.10.12.5  PROCESS  CONTROL 
Not  applicable. 


3.3.2.3.10.12.6  PROCESSING 

The  following  describes  the  processing  performed  by  this  part: 

separate  (Wander_Azimuth_Navigation_Parts) 
function  Compute_Wander_Azimuth_Angle 

(DC_P_NE  :  Direction_Cosine_Matrix_Elements; 
DC_P_NN  :  Direction_Cosine_Matrix_Elements) 
return  Wander_Angles  is 


begin 

return  Arctan(DC_P_NE  /  DC_P_NN); 
end  Compute_Wander_Azimuth_Angle; 


3.3.2.3.10.12.7  UTILIZATION  OF  OTHER  ELEMENTS 
None. 


3.3.2.3.10.12.8  LIMITATIONS 


None. 


3.3.2.3.10.13  COMPUTE j:AST_VELOCITY_yiTH_SIN_COS_IN  UNIT  DESIGN  (CATALOG  #P1099-0) 

This  function  computes  the  east  velocity  given  the  nominal  east  and  north 
velocities  and  the  wander  angle. 

The  computations  are  performed  as  follows: 

VelE  :  =  VelNE  *  Cos(VA)  -  VelNN  *  Sin(WA) 

where  VelNE  *  nominal  east  velocity 
VelNN  a  nominal  north  velocity 
WA  =  wander  angle 


3.3.2.3.10.13.1  REQUIREMENTS  ALLOCATION 
N/A 


3.3.2.3.10.13.2  LOCAL  ENTITIES  DESIGN 


None. 


CAMP  Software  Detailed  Design  Document 


Page  176 


3.3.2.3.10.13.3  INPUT/OUTPUT 
GENERIC  PARAMETERS: 

The  following  generic  parameters  were  previously  defined  for  this  part's 
specification  in  the  Wander_Azimuth_Navigation_Parts  package  specification: 

Data  types: 

The  following  table  describes  the  generic  formal  types  required  by  this  part: 


|  Name 

Type 

Description  | 

|  Sin  Cos  Ratio 

I  "  " 

|  Velocities 

1 

floating 
point  type 
floating 
point  type 

Data  type  of  results  from  a  sine  or  cosine  | 
operation  j 
Data  type  of  velocity  measurements  j 

1 

Subprograms: 

The  following  table  describes  the  generic  formal  subroutines  required  by  this 
part: 


|  Name 

Type 

Description  | 

w 

o 

°l  i 
s 

1 

to 

1 

—  1 

procedure 

Returns  the  sine  and  cosine  of  a  wander  angle  | 

FORMAL  PARAMETERS: 

The  following  table  describes  this  part's  formal  parameters: 


|  Name 

Type 

|  Mode 

j  Description 

1 

Nominal  East 

Velocities 

1  In 

|  Velocity  which  would  be  east  if 

the| 

Velocity 

S  wander  angle  was  0 

Nominal  North 

Velocities 

1  In 

j  Velocity  which  would  be  north  if 

Velocity 

j  the  wander  angle  was  0 

Sine  of 

Sin  Cos 

1  In 

j  Sine  of  the  current  wander 

Wander  Angle 

Ratio 

j  azimuth  angle 

Cosine  of 

Sin  Cos 

1  In 

j  Cosine  of  the  current  wander 

Wander_Angle 

Ratio 

1 

j  azimuth  angle 

3.3.2.3.10.13.4  LOCAL  DATA 
Data  objects: 

The  following  table  describes  the  data  objects  maintained  by  this  part: 
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|  Name 

1  Type  | 

Description  | 

|  Answer 

|  Velocities  | 

East  velocity  being  calculated  | 

3.3.2.3.10.13.5 

PROCESS  CONTROL 

Not  applicable. 

3.3.2.3.10.13.6 

PROCESSING 

The  following  describes  the  processing  performed  by  this  part: 

separate  (Wander_Azimuth  Navigation_Parts) 
function  Compute_East_VeIocity_with_Sin_Cos  In 

(NomlnalJBast  Velocity  :  Velocities; 

Nominal_NortK_Velocity  :  Velocities; 

Sine_of_Wander_Angle  :  Sin_Cos_Ratio; 

Cosine  of  Wander  Angle  :  Sin  Cos  Ratio)  return  Velocities  is 


- declaration  of  variables- 


Ansver  :  Velocities; 


— begin  function  Compute_East_Velocity 


begin 

Answer  :=  Nominal_East  Velocity  *  Cosine  of_Wander_Angle  - 
Nominal_NortE_Velocity  *  Sine_of_Wander_Angle; 

return  Answer; 

end  Compute_Eas t_Veloci ty_wi th_3in_Cos_In ; 


3.3.2.3.10.13.7  UTILIZATION  OF  OTHER  ELEMENTS 
None. 

3.3.2.3.10.13.8  LIMITATIONS 


1 


■ti 


None. 
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3.3.2.3.10.14  C0MPUTE_N0RTH_VEL0CITY_WITH_SIN_C0S_IN  UNIT  DESIGN  (CATALOG 
#P1101-0) 

This  function  calculates  the  North  velocity  component  of  a  missile  given  the 
nominal  north  and  east  velocities  of  the  missile. 


The  computations  are  performed  as  follows: 
VelN  :=  VelNE  *  Sin(VA)  +  VelNN  *  Cos(WA) 


where  VelNE  =  nominal  east  velocity 
VelNN  =  nominal  north  velocity 
WA  =  wander  angle 


3.3.2.3.10.14.1  REQUIREMENTS  ALLOCATION 
N/A 


3.3.2.3.10.14.2  LOCAL  ENTITIES  DESIGN 
None. 

3.3.2.3.10.14.3  INPUT/OUTPUT 


GENERIC  PARAMETERS: 

The  following  generic  parameters  were  previously  defined  for  this  part's 
specification  in  the  Wander_Azimuth_Navigation_Parts  package  specification: 

Data  types: 

The  following  table  describes  the  generic  formal  types  required  by  this  part: 


|  Name 

Type 

Description  | 

Sin_Cos_Ratio 

Velocities 

floating 
point  type 
floating 
point  type 

Data  type  defining  results  of  a  sine  or  | 

cosine  operation  j 

Data  type  defining  velocity  measurements  j 

1 

Subprograms: 

The  following  table  describes  the  generic  formal  subroutines  required  by  this 
part: 


|  Name 

1  Type 

|  Description  | 

| 

1 

|  function 

1 

|  Multiplication  operator  defining  the  operation:  | 

j  Velocities  *  Sin_Cos_Ratio  =>  Velocities  j 
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FORMAL  PARAMETERS: 

The  following  table  describes  this  part's  formal  parameters: 


|  Name  |  Type  |  Mode  |  Description 


Nominal  East 

|  Velocities 

1 

In 

Velocity 

1 

Nominal  North 

j  Velocities 

In 

Velocity 

1 

1 

Sine  of 

|  Sin  Cos 

1 

1 

In 

Wander  Angle 

j  Ratio 

1 

Cosine  of 

|  Sin  Cos 

1 

In 

Wander  Angle 

j  Ratio 

1 

Velocity  in  the  direction  which 
would  be  East  if  the  wander  angle 
was  0 

Velocity  in  the  direction  which 
would  be  North  if  the  wander  angle 
was  0 

Sine  of  the  current  wander 
azimuth  angle 

Cosine  of  the  current  wander 
azimuth  angle 


3.3.2.3.10.14.4  LOCAL  DATA 
Data  objects: 

The  following  table  describes  the  data  objects  maintained  by  this  part: 

|  Name  |  Type  |  Description  | 

|  Answer  |  Velocities  |  North  velocity  being  calculated  j 


3.3.2.3.10.14.5  PROCESS  CONTROL 
Not  applicable. 

3. 3. 2. 3. 10. U.6  PROCESSING 

The  following  describes  the  processing  performed  by  this  part: 

separate  (Wander_Azimuth_Nav.igation_Parts) 
function  Compute  North_Veloci.ty  with_Sin  Cos_In 
(NomInal_East  Velocity  :  Velocities; 

Nominal _NortE_Velocity  :  Velocities; 

Sine_o£”Wander_Angle  :  Sin_Cos_Ratio; 

Cosine_of_Wander_Angle  :  Sin_Cos_Ratio)  return  Velocities  is 
—  — declaration  of  variabies- 
Answer  :  Velocities; 

— begin  function  Compute_North_Velocity 
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begin 

Answer  :=  Nominal_East_Velocity  *  Sine_of_Wander_Angle  + 
Nominal_North_Velocity  *  Cosine_of_Wander_Angle; 

return  Answer; 

end  Compute_North_Veloci ty_wi th_Sin_Cos_In ; 


3.3.2.3.10.14.7  UTILIZATION  OF  OTHER  ELEMENTS 
None. 

3.3.2.3.10.14.8  LIMITATIONS 
None. 


3.3.2.3.10.15  COMPUTE_EARTH_RELATIVE_HORIZONTAL_VELOCITIES  WITH  SIN  COS  IN  UNIT 
DESIGN  (CATALOG  #Pll07-0)  "  “  ~  “ 

This  procedure  computes  the  north  and  east  velocity  given  the  nominal  east  and 
north  velocities  and  the  wander  angle. 

The  computations  are  performed  as  follows: 

VelE  :-  VelNE  *  Cos(WA)  -  VelNN  *  Sin(WA) 

VelN  :=•  VelNE  *  Sin(WA)  +  VelNN  *  Cos(WA) 

where  VelNE  =  nominal  east  velocity 
VelNN  =  nominal  north  velocity 
WA  =  wander  angle 


3.3.2.3.10.15.1  REQUIREMENTS  ALLOCATION 
N/A 

3.3.2.3.10.15.2  LOCAL  ENTITIES  DESIGN 
None. 


3.3.2.3.10.15.3  INPUT/OUTPUT 
GENERIC  PARAMETERS: 

The  following  generic  parameters  were  previously  defined  for  this  part's 
specification  in  the  Wander_Azimuth_Navigation_Parts  package  specification: 
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Data  types: 

The  following  table  describes  the  generic  formal  types  required  by  this  part: 


|  Name 

Type 

Description  | 

|  Sin  Cos  Ratio 

1 

|  Velocities 

1 

floating 
point  type 
floating 
point  type 

Data  type  defining  results  of  a  sine  or  j 

cosine  operation  | 

Data  type  defining  velocity  measurements  j 

1 

Subprograms : 

The  following  table  describes  the  generic  formal  subroutines  required  by  this 
part: 


|  Name 

1  Type 

|  Description  | 

| 

1 

|  function 

1 

[  Multiplication  operator  defining  the  operation:  1 

j  Velocities  *  Sin_Cos_Ratio  =>  Velocities  j 

FORMAL  PARAMETERS: 

The  following  table  describes  this  part's  formal  parameters: 


|  Name 

1  Type  | 

Mode 

|  Description  | 

|  Nominal_East_ 
j  Velocity 

Velocities  | 

1 

1 

In 

|  Velocity  in  the  direction  that  j 
i  would  be  East  if  the  wander  i 
j  angle  was  0  j 

j  Nominal_North_ 
j  Velocity 

Velocities  ] 

1 

In 

|  Velocity  in  the  direct  that  would  | 

!  be  North  if  the  wander  angle  was  j 
O  i 

j  Current^ 

|  Wander~Angle 

I 

Wander_Angles | 

In 

|  Value  of  the  current  wander  angle  j 
1  1 

j  Sine  of_ 

1  Wan3er-Angle 

Sin_Cos_  j 

Ratio  j 

In 

|  Sine  of  the  current  wander  1 

i  azimuth  angle  j 

j  Cosine_of_ 
j  Wander  Angle 

Sin_Cos_  j 

Ratio  j 

In 

j  Cosine  of  the  current  wander  j 

!  azimuth  angle  j 

j  East_VeIocity 

Velocities  i 

1 

Out 

j  Current  east  velocity  of  the  j 

|  missile  j 

j  North_Velocity 

Velocities  j 

1 

Out 

j  Current  north  velocity  of  the  j 

j  missile  j 

3.3.2.3.10.15.4  LOCAL  DATA 


None. 
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3.3.2.3.10.15.5  PROCESS  CONTROL 
Not  applicable. 


3.3.2.3.10.15.6  PROCESSING 

The  following  describes  the  processing  performed  by  this  part: 
separate  (Wander_Azimuth_Navigation_Parts) 

procedure  Compute_Earth_Relative_Horizontal_Velocities_with_Sin_Cos_In 
(Nominal_East_Velocity  :  in  Velocities; 

Nominal_North_Velocity  :  in  Velocities; 

Sine_of_Wander_Angle  :  in  Sin_Cos_Ratio; 

Cosine_of_Wander_Angle  :  in  Sin_Cos  Ratio; 

East  Velocity  :  out  Velocities; 

NortK_Velocity  :  out  Velocities)  is 

begin 

East_Velocity  :=  Nominal_East_Velocity  *  Cosine_of_Wander_Angle  - 
Nominal  North_Velocity  *  Sine_of_Wander_Angle; 
North_Velocity  :=  NominaI_East  Velocity  *  Sine  Of_Wander  Angle  + 
Nominal_NortE_Velocity  *  CosIne_of_VaniJer_Angle; 

end  ^ompute_Earth_Relative_Horizontal_Velocities_with_Sin_Cos_In; 


3.3.2.3.10.15.7  UTILIZATION  OP  OTHER  ELEMENTS 
None. 


3.3.2.3.10.15.8  LIMITATIONS 
None. 


3.3.2.3.10.16  COMPUTE  LATITUDE  USING  TWO  VALUE  ARCTANGENT  UNIT  DESIGN  (CATALOG 
#P1104-0)  "  " 

This  function  computes  the  current  Latitude  given  some  elements  of  the 
Direction  Cosine  matrix. 

The  computations  are  performed  as  follows: 

Lat  :=  Arctan(DCPV  /  SqRT(DCPNE**2  +  DCPNN**2) 

(see  formal  parameters  section  for  explanation  of  abbreviations) 


3.3.2.3.10.16.1  REQUIREMENTS  ALLOCATION 
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3.3.2.3.10.16.2  LOCAL  ENTITIES  DESIGN 
None. 

3.3.2.3.10.16.3  INPUT/OUTPUT 
GENERIC  PARAMETERS: 

The  following  generic  parameters  were  previously  defined  for  this  part's 
specification  in  the  Vander_Azimuth_Navigation_Parts  package  specification: 

Data  types: 

The  following  table  describes  the  generic  formal  types  required  by  this  part: 


|  Name  |  Type  |  Description 

|  Earth_Position  |  floating  |  Data  type  used  to  define  latitude  and 

I  j  point  type  j  longitude  measurements 

j  Direction  Cosine_  j  floating  j  Data  type  used  to  define  elements  of  a 

j  Matrix_Elements  j  point  type  j  direction  cosine  matrix 


Subprograms : 

The  following  table  describes  the  generic  formal  .'ubroutines  required  by  this 
part: 


|  Name 

Type 

Description  | 

|  Arctan2 

1  Sqrt 

function 

function 

4-quadrant  arctangenc  function  | 

Square  root  function  j 

FORMAL  PARAMETERS: 

The  following  table  describes  this  part's  formal  parameters: 


Name 

1 

Type 

|  Mode 

|  Description 

DC  P  V 

1 

Direction  Cosine 

1  In 

|  Sine  of  the  current  latitude  of  the 

1 

Matrix  Elements 

j  missile 

DC  P  NE 

1 

Direction  Cosine 

1  In 

|  Product  of  the  sine  of  the  wander 

1 

Matrix_Elements 

j  angle  and  the  cosine  of  the 
|  latitude 

DC  P  NN 

1 

Direction  Cosine 

1  In 

Product  of  the  cosine  the  wander 

Matrix  Elements 


angle  and  the  cosine 
latitude 


•r.*  .  •'w 
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3.3.2.3.10.16.4  LOCAL  DATA 
Data  objects: 

The  following  table  describes  the  data  objects  maintained  by  this  part: 


|  Name  | 

Type 

|  Description  | 

|  Answer  | 

|  Denominator  j 

1  1 

Earth_Positions 
Direction  Cosine 
Matrix  Elements 

|  Latitude  being  calculated  | 
j  Temporary  variable  used  to  hold  results  | 
j  of  intermediate  calculations  j 

3.3.2.3.10.16.5 

PROCESS  CONTROL 

Not  applicable. 

’ 

3.3.2.3.10.16.6 

PROCESSING 

The  following  describes  the  processing  performed  by  this  part: 

separate  (Wander_Azimuth_Navigation_Parts) 
function  Compute_Latitude_Using_Tvo_Value_Arc tangent 

(DC_P_V  :  Direction_Cosine_Matrix_Elements; 
DC_P_NE  :  Direction_Cosine_Matrix_Elements; 
DC_P_NN  4  Direction_Cosine_Matrix_Elements) 
return  Earth  Positions  is 


- declaration  of  variables- 


Answer  :  Earth_Positions; 

Denominator  :  Direction  Cosine  Matrix  Elements; 


— beginning  of  function- 
begin 


Denominator  :=  Sqrt(  DC_P_NE  *  DC_P_NE  + 

DC_P_NN  *  DC_P_NN  ); 

Answer  :=  Arctan2  (Y  =>  DC_P_V, 

X  =>  Denominator); 

return  Answer; 

end  Compute_Latitude_Using_Two_Value_Arctangent; 
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3.3.2.3.10.16.7  UTILIZATION  OF  OTHER  ELEMENTS 


None. 


3.3.2.3.10.16.8  LIMITATIONS 
None. 


3.3.2.3.10.17  C0MPUTE_L0NGITUDE_USING_TW0_VALUE_ARCTANGENT  UNIT  DESIGN  (CATALOG 
#P1106-0) 

This  function  computes  the  current  longitude  given  some  elements  of  the 
direction  cosine  matrix. 


3.3.2.3.10.17.1  REQUIREMENTS  ALLOCATION 
N/A 


3.3.2.3.10.17.2  LOCAL  ENTITIES  DESIGN 
None. 


,£  3.3.2.3.10.17.3  INPUT/OUTPUT 

GENERIC  PARAMETERS: 

The  following  generic  parameters  were  previously  defined  for  this  part's 
specification  in  the  Wander_AzirauthJNavigation_Parts  package  specification: 

Data  types: 

The  following  table  describes  the  generic  formal  types  required  by  this  part: 


|  Name 

1  Type 

|  Description 

1 

|  Earth  Position 

|  floating 

|  Data  type  used  to  define 

latitude  and  j 

i 

|  point  type 

|  longitude  measurements 

1 

|  Direction  Cosine 

j  floating 

j  Data  type  used  to  define 

elements  of  a  | 

j  Matrix_Elements 

j  point  type 

j  direction  cosine  matrix 

1 

Subprograms : 

The  following  table  describes  the  generic  formal  subroutines  required  by  this 
part: 


|  Name  | 

Type 

Description 

1 

|  Arctan2| 

function 

4-quadrant  arctangent  function 

1 
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FORMAL  PARAMETERS: 

The  following  table  describes  this  part's  formal  parameters: 


|  Name  |  Type 


|  Description 


DC  R  V 


DC  G  V 


Direction_Cosine 

Matrix_Elements 

Direct ion_Cosine_ 
Matrix  Elements 


Product  of  the  cosine  of  the  current 
latitude  and  the  sine  of  the 
current  longitude 

Product  of  the  cosine  of  the  current 
latitude  and  the  cosine  of  the 
current  longitude 


3.3.2.3.10.17.4  LOCAL  DATA 
None. 

3.3.2.3.10.17.5  PROCESS  CONTROL 
Not  applicable. 

3.3.2.3.10.17.6  PROCESSING 

The  following  describes  the  processing  performed  by  this  part:  * 

separate  (Wander_Azimuth  Navigation_Parts) 
function  ComputeJLongi tucIe_using_Two_Value_Arc tangent 
(DC_R_V  :  Direction_Cosine_Matrix_Elements; 

DC_G_V  :  Direction_Cosine_Matrix_Elements) 
return  Earth_Positions  is 

—  — declaration  section 

Answer  :  Earth_Positions; 

— begin  function 

begin 

return  Arctan2  (Y  =>  DC_R_V, 

X  =>  DC_G_V) ; 

end  Compute_Longi tude_using_Two_Value_Arctangent ; 
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3.3.2.3.10.17.7  UTILIZATION  OF  OTHER  ELEMENTS 
None. 

3.3.2.3.10.17.8  LIMITATIONS 
None. 

3.3.2.3.10.18  COMPUTE JfANDER_AZIMUTH_ANGLEJJSINGJTWO_VALUE_ARCTANGENT  UNIT  DESIGN 
(CATALOG  #P1 109-0) 

This  function  computes  the  current  wander  azimuth  angle  given  some  elements  of 
the  Direction  Cosine  matrix. 

The  computations  are  performed  as  follows: 

WA  :=  Arctan(DCPNE  /  DCPNN) 

(see  formal  parameters  section  for  explanation  of  abbreviations) 


3.3.2.3.10.18.1  REQUIREMENTS  ALLOCATION 


This  part  meets  CAMP  requirement  R028 

3.3.2.3.10.18.2  LOCAL  ENTITIES  DESIGN 
None. 

3.3.2.3.10.18.3  INPUT/OUTPUT 
GENERIC  PARAMETERS: 

The  following  generic  parameters  were  previously  defined  for  this  part's  | 

specification  in  the  Wander_Azirauth_Navigation_Parts  package  specification:  | 

Data  types:  | 


The  following  table  describes  the  generic  formal  types  required  by  this  part: 


|  Name 

Type 

Description  | 

|  Direction_Cosine_ 

|  Matrix_Elements 
j  Wander  Angles 

1 

floating 
point  type 
floating 
point  type 

Data  type  defining  elements  in  a  | 

direction  cosine  matrix  ] 

Data  type  defining  wander  angle  1 

measurements  j 

Subprograms: 


•J'  v*  ■ 


*  v  -*  V 
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The  following  table  describes  the  generic  formal  subroutines  required  by  this 
part: 


|  Name  | 

Type 

Description  j 

|  Arctan2 | 

function 

4-quadrant  Arctangent  function  | 

FORMAL  PARAMETERS: 

The  following  table  describes  this  part's  formal  parameters: 


|  Name  |  Type 


DC_P_NE  |  Direction_Cosine_ 
;  Matrix  Elements 


Mode  |  Description 
In 


I 


DC_P_NN  |  Direction_Cosine_ 
Matrix  Elements 


In 


Product  of  the  sine  of  the  wander 
angle  and  the  cosine  of  the 
latitude 

Product  of  the  cosine  of  the  wander 
angle  and  the  cosine  of  the 
latitude 


3.3.2.3.10.18.4  LOCAL  DATA 
Data  objects: 

The  following  table  describes  the  data  objects  maintained  by  this  part: 


|  Name 

1  Type 

|  Description  | 

|  Answer 

|  Wander_Angles 

|  Wander  angle  being  calculated  | 

3.3.2.3.10.18.5  PROCESS  CONTROL 
Not  applicable. 


3.3.2.3.10.18.6  PROCESSING 

The  following  describes  the  processing  performed  by  this  part: 


separate  (Wander_Azimuth_Navigation_Parts) 

function  Compute_Wander_Azimuth_Angle_using_Two_Value_Arctangent 
(DC_P_NE  :  Direction_Cosine_Matrix_Elements? 

DC_P_NN  :  Direction_Cosine_Matrix_Elements) 
return  Wander  Angles  is 


—  — declaration  section 


© 


U.-V_k-D  'j 
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Answer  :  Wander_Angl.es ; 

— begin  function 
begin 

Answer  :=  Arctan2  (Y  =>  DC_P_NE , 

X  =>  DC_P_NN) ; 

return  Answer; 

end  Compute_Uander_A2imuth_Angle_using_Two_Value_Arc tangent 

3.3.2.3.10.18.7  UTILIZATION  OF  OTHER  ELEMENTS 
None. 


3.3.2.3.10.18.8  LIMITATIONS 
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package  body  Wander  Azimuth  Navigation  Parts  is 


function  Compute_East_Velocity 

(Nominal_East_Velocity  :  Velocities; 
Nominal_North_Velocity  :  Velocities; 
Current_Wander_Angle  :  Wander_Angles) 
return  Velocities  is  separate; 


function  Compu t e_Eas  t_Veloci ty_Wi th_Sin_Cos_In 

(Nominal_East_Velocity  :  Velocities; 
Nominal_North_Velocity  :  Velocities; 
Sine_Of_Wander_Angle  :  Sin_Cos_Ratio; 
Cosine_Of_Wander_Angle  :  Sin_Cos_Ratio) 
return  Velocities  is  separate; 


function  Compute_North_Velocity 

(Nominal_East  Velocity  s  Velocities; 
Nominal_NortK_Velocity  :  Velocities; 
Current_Wander_Angle  :  Wander_Angles) 
return  Velocities  is  separate; 


fimction  Compute_North_Velocity_With_Sin  Cos_In 
(Nominal_East_Velocity  :  Velocities; 
Nominal_North_Velocity  :  Velocities; 
Sine_Of_Wander_Angle  i  Sin_Cos_Ratio; 
Cosine_Of_Vander_Angle  :  Sin~Cos_Ratio) 
return  Velocities  is  separate; 


procedure  Compu te_Earth_Relative  Horizontal_Velocities 
(Nominal_East  Velocity  :  in  Velocities; 
Nominal_Nor tK_Veloci ty 
Current  Wandar_Angle 
East  Velocity 
NortK_Velocity 
is  separate; 


in  Velocities; 
in  Wander_Angles ; 
out  Velocities; 
out  Velocities) 


procedure  Compute  Earth  Relative  Horizontal  Velocities  With  Sin  Cos  In 


(Nominal_East  Velocity 
Nominal_NortK_Velocity 
Sine_Of_Wander_Angle 
Cosine_Of_Wander_Angle 
East  Velocity 
NortK_Velocity 
is  separate; 


in  Velocities; 
in  Velocities; 
in  Sin_Cos_Ratio; 
In  Sin_Cos  Ratio; 
out  Velocities; 
out  Velocities) 


function  Compute_Total_Angular_Veloci ty 

(Rho_East  :  Angular_Velocities; 
Rho_North  :  Angular  Velocities) 
return  Angular  Velocities  is  separate; 


function  Compu te_Coriolis_Accelerat ion 

(Velocity_In  :  Velocity_Vectors; 

Rho_In  :  Angular_Velocity_Vectors; 
Omega_In  :  Angular_Velocity_Vectcrs) 
return  Acceleration  Vectors  is  separate; 


package  body  Coriolis_Acceleration_From_Total_Rates  is  separate; 


■A 
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function  Compute_Curvatures 

(Dc_P  V  :  Direction_Cosine_Matrix_Elements; 

Dc_P~Ne  :  Direction_Cosine_Matrix_Elements; 

Dc_P_Nn  :  Direction_Cosine_Matrix_Elements; 

Current_Altitude  :  Distances) 

return  Inverse_Distance_Vectors  is  separate; 

function  Total_Platform_Rotation  Rate 

(REo_Vector  :  AnguIar_Velocity_Vectors; 

Omega_Vector  :  Angular_Velocity_Vectors) 
return  Angular_Velocity_Vectors  is  separate; 

package  body  Earth_Rotation_Rate  is  separate; 

function  Compute_Earth_Relative_Navigation_Rotation_Rate 
(Missile_Velocity  :  Velocity_Vectors; 

Curvatures  :  Inverse_Distance_Vectors) 

return  Angular_Velocity_Vectors  is  separate; 

function  Compute_Latitude  (Sin_Latitude  :  Sin_Cos_Ratio) 

return  Earth_Positions  is  separate; 

function  Compute_Lati tude_Using_Arc tangent 

(Dc_P_V  :  Direction_Cosine_Matrix_Elements; 

Dc_P_Ne  :  Direction_Cosine_Matrix_Elements; 

Dc_P_Nn  :  Direction_Cosine_Matrix_Elements) 
return  EarthPositions  is  separate; 

function  ComputeLati tude_Using_Tvo_Value_Arctangent 

(Dc_P_V  :  Direction_Cosine_Matrix_Elements; 

Dc_P_Ne  :  Direction_Cosine_Matrix_Elements; 

Dc_P_Nn  :  Direction_Cosine_Matrix_Elements) 
return  Earth_Positions  is  separate; 

function  Compute_Longitude 

(Dc_R_V  :  Direction_Cosine_Matrix_Elements; 

Dc_G_V  :  Direction_Cosine_Matrix_Elements) 
return  Earth_Positions  is  separate; 

function  Compute_Longitude_Using_Two_Value_Arc tangent 
(Dc_R_V  :  Direction_Cosine_Matrix_Elements; 

Dc_G_V  :  Direction_Cosine~Matrix_Elements) 
return  Earth_Positions  is  separate; 

f unc tion  Comput e_Vand e r_Az i mu t h_Angle 

(Dc_P_Ne  :  Direction_Cosine_Matrix_Elements; 

Dc_P_Nn  :  Direction_Cosine_Matrix_Elements) 
return  Wander_Angles  is  separate; 

function  Compute_Wander_Azimuth_Angle_Using_Two_Value_Arc tangent 
(Dc_P_Ne  :  Direction_Cosine_Matrix_Elements; 

Dc_P_Nn  :  Direction_Cosine_Matrix_Elements) 
return  Wander_Angles  is  separate; 

end  Wander_Azimuth_Navigation_Parts; 
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separate  (Wander_Azimuth_Navigation_Parts) 
function  Compute_East_Velocity 

(Nominal_East_Velocity  :  Velocities; 

Nominal_North_Velocity  :  Velocities; 

Current_Wander_Angle  :  Wander_Angi.es)  return  Velocities  is 


—  — declaration  of  variables- 


Cos_Of_Angle  :  Sin_Cos_Ratio; 
Sin_Of_Angle  :  Sin_Cos_Ratio; 


—  begin  Junction  Compute  East  Velocity 

begin 

S in_Cos ( Cur ren t_Wander_Angle ,  Sin_Of_Angle,  Cos_Of_Angle) ; 

return  (  Nominal_East_Velocity  *  Cos_0f  Angle  - 
Nominal_NortH_Velocity  *  Sin_0?_Angle) ; 

end  Compu te_Eas  t_Veloci ty ; 


I 

\ 
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separate  ( Wander_Azimu  th_Naviga  t i on_Par  t s ) 
function  Compute_North_Velocity 

(Nominal_East_Velocity  :  Velocities; 

Nominal_North_Velocity  :  Velocities; 

Current_Wander_Angle  :  Wander_Angles)  return  Velocities  is 


—  —  declaration  of  variables- 


Cos_Of_Angle  :  Sin_Cos_Ratio; 
Sin_Of_Angle  :  Sin_Cos  Ratio; 


— begin  function  Compute  North  Velocity 


begin 

Sin_Cos(Current_Wander_Angle,  Sin_Of_Angle,  Cos_Of_Angle) ; 

return  (  Nominal_East_Velocity  *  Sin_0f  Angle  + 
Norainal_North_Velocity  *  Cos_Ol_Angle); 

end  Compute_North_Velocity; 


k."  vw'  >C'  o  *c"  n'  v.  v  v\v\v  v.imH'yvvy v  .v  v.v.v.x,w  kak." 
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separate  ( Wander_Az imu th_Nav iga t i on_Far t s ) 
procedure  Compute_Earth_Relative_Horizontal_Velocities 
(Nominal_East_Velocity  :  in  Velocities; 
Nominal_North_Velocity 
Current_Wander_Angle 
Eas t_Velocity 
North  Velocity 


in  Velocities; 
in  Wander_Angles ; 
out  Velocities; 
out  Velocities)  is 


—  —  declaration  of  var'tables- 


Cos_Of_Angle  :  Sin_Cos_Ratio; 
Sin_Of_Angle  :  Sin_Cos_Ratio; 


— begin  procedure  Compute  Earth  Relative  Horizontal  Velocities 


begin 

Sin_Cos(Current_Wander_Angle,  Sin_Of_Angle,  Cos_Of_Angle) ; 

East_Velocity  :=  Norai.nal_East  Velocity  *  Cos_0f  Angle  - 
Nominal  NortE_Velocity  *  Sin_Ol_Angle; 
North_Velocity  :  =  NominaI_East  Velocity  *  Sin_0f  Angle  + 
Nominal_NortE_Velocity  *  Cos_Or_Angle: 

end  Compute_Earth_Relative_Horizontal_Velocities; 
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separate  (Wander_Azimuth_Navigation_Parts) 
function  Compu  te_To  tal_Angular_Veloci ty 

(Rho_East  :  Angular_Velocities; 

Rho_North  :  Angular_Velocities)  return  Angular_Velocities  is 


begin 

return  Sqrt(Rho_East  *  Rho_East  +  Rho_North  *  Rho_North); 
end  Compu  te_To  tal_Angular_Veloci ty ; 


1 

\ 

i 

3 


l 


$ 
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separate  (Wander_Azimuth_Navigation_Parts) 
function  Compute_Coriolis_Acceleration 

(Velocity_In  :  Velocity_Vectors; 

Rho_In  :  Angular_Velocity_Vectors; 

Omega_In  :  Angular_Velocity_Vectors) 
return  Acceleration  Vectors  is 


—  — declaration  of  variables- 

Temp_A_V_V  :  Angular_Velocity_Vectors; 

— begin  function  Compute  Coriolis  Acceleration 

begin 

Temp_A_V_V  :=  Omega_In  +  Omega  In; 

Temp_A_V_V  :=  Sparse_Right_Z_Acfd(Temp_A_V_v ,  Rho_In); 

return  Cross_Product(Terap_A_V_V,  Velocity_In); 

end  Compute_Coriolis_Acceleration; 


*-kn‘ 
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separate  (Wander_A2imuth_Navigation_Parts) 

package  body  Coriolis_Acceleration_From_Total_Rates  is 


—  —  declaration  of  variables- 


E  :  constant  Indice  :=  Indices' FIRST; 

N  :  constant  Indices  :=  Indices 'SUCC(E); 

U  :  constant  Indices  :=  Indices' LAST; 

pragma  PAGE; 


—  — unit  body- 


function  Compute  (Velocity_In  :  Velocity_Vectors; 

Omega  :  Angular_Velocity_Vectors; 
Total_Rates  :  Angular_Velocity_Vectors) 
return  Acceleration  Vectors  is 


—  declaration  of  variables- 


Temp_A_V_V  :  Angular_Velocity_Vectors; 


—  — begin  function  Compute 


begin 

Temp_A_V_V(E)  :=  Oraega(E)  +  Total_Rates(E) ; 

Temp_A_V_V(N)  :=  Omega(N)  +  Total_Rates(N) ; 

Temp_A_V_V(U)  :=  Omega(U)  +  Omega(U); 

return  Cross_Product(Temp_A_V_V,  Velocity_In); 

end  Compute; 

end  Coriolis  Acceleration  From  Total  Rates; 
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separate  (Wander_Azimuth_Navigation_Parts) 
function  Compute_Curvatures 

(Dc_P_V  :  Diraction_Cosine_Matrix_Elements; 

Dc_P_Ne  :  Direction_Cosine_Matrix_Elements; 

Dc_P  Nn  :  Direction_Cosine_Matrix_Elements; 

Currint_Altitude  :  Distances) 

return  Inverse  Distance  Vectors  is 


—  — declaration  of  variables- 


Denominator 

Distance_Ratio 

Partial_Denominator 

Curvatures 

Two  Flat  X  Dc  P  Nn 


Real, 

Real? 

Real; 

Inverse_Distance_Vectors; 

Real; 


— beginning  of  function- 


begin 

Distance_Ratio  :=  Current_Altitude  *  One_Over_Earth_Radius 

Partial_Denominator  :=*  1.0  - 

DistanceRatio  - 
Earth_Flattening_Coefficient  * 

(Dc_P_V  *  Dc_P_V); 

Denominator  :=  Partial  Denominator  + 

Two_Flat  *  (Dc_P^Na  *  Dc_P_Ne); 
Curvatures(Na)  :=  0ne_0ver_Earth_RadIus  *  Denominator; 

Two_Flat_X_Dc_P_Nn  :=  Two_Flat  *  Dc_P_Nn; 

Denominator  :=  Partial  Denominator  + 

Two_Flat_X_Dc_P  Nn  *  Dc_P_Nn; 

Curvatures(Nn)  :=  0ne_0ver_Earth_Racfius  *  Denominator; 

Denominator  :=  Two_Flat_X_Dc  P_Nn  *  Dc_P_Ne; 

Curvatures (V)  :=  0ne_0ver_EartK_Radius  *  Denominator; 

return  Curvatures; 


end  Compute_Curvatures; 
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separate  (Wander_Azimuth_Navigation_Parts) 
function  Total_Platform_Rotation_Rate 

(Rho_Vector  :  Angular_Velocity_Vectors; 
Omega_Vector  :  Angular_Velocity_Vectors) 
return  Angular_Velocity_Vectors  is 

begin 

return  Sparse_Right_Z_Add(Lef t  =>  Omega_Vector, 

Right  =>  Rho_Vector); 
end  Total  Platform  Rotation  Rate; 
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separate  ( Wander_Az imu  t h_Naviga  t i on_Par  t  s ) 
package  body  Earth  Rotation  Rate  is 


—  —  local  declarations 


:  constant  Indices  :=  Indices' FIRST; 

:  constant  Indices  :=  Indices' SUCC(Ne); 

:  constant  Indices  :=  Indices' LAST; 


pragma  PAGE; 


—  — unit  body- 


function  Compute  (Dc_P_V  :  Direction_Cosine_Matrix_Elements; 

Dc_P_Ne  :  Direction_Cosine_Matrix_Elements; 
Dc_P_Nn  :  Direction_Cosine_Matrix_Elements) 
return  Angular  Velocity  Vectors  is 


— declaration  of  variables- 


Rotation_Rate  :  Angular_Velocity_Vectors; 


—  — beginning  of  function- 


begin 


Rotation_Rate(Ne) 
Rotation_Rate(Nn) 
Rotation  Rate(V) 


=  Dc_PJNe  *  Earth_Rate; 
=  Dc_P_Nn  *  Earth_Rate; 
=  Dc  P  V  *  Earth  Rate; 


return  Rotation  Rate; 


end  Compute; 


end  Earth  Rotation  Rate; 
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separate  (Wander_Azimuth_Navigation_Parts) 

function  Compute_Latitude  (Sin_Latitude  :  Sin_Cos_Ratio) 

return  Earth_Positions  is 

begin 

return  Arcsin(Sin_Latitude) ; 
end  Compute_Latitude; 
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separate  ( Wander_Az imu  t h_Naviga  t i on_Par  ts ) 
function  Compute_Latitude_Using_Arc tangent 

(Dc_P_V  :  Direction_Cosine_Matrix_Elements; 
Dc_P_Ne  :  Direction_Cosine_Matrix_Elements; 
Dc_P_Nn  :  Direction_Cosine_Matrix_Elements) 
return  Earth  Positions  is 


—  — declaration  of  variables- 


Denominator  :  Direction  Cosine  Matrix  Elements; 


— beginning  of  funclion- 


begin 


I 


Denominator  :=  Sqrt(  Dc_P_Ne  *  Dc_P_Ne  + 

Dc  P  Nn  *  Dc  P  Nn  ); 


return  Arctan(Dc_P_V  /  Denominator); 
end  Compute_Lati tude_Using_Arctangent ; 
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separate  (Wander_Azimuth_Navigation_Parts) 

function  ComputeJLongitude  (Dc_R_V  :  Direction_Cosine_Matrix_Elements; 

Dc_G_V  :  Direction_Cosine_Matrix_Elements) 
return  Earth_Positions  is 

begin 

return  Arctan(Dc_R_V  /  Dc_G_V); 


end  Compute  Longitude; 
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separate  (Wander_Azimuth_Navigation_Parts) 
function  Compute_Vander_Azimuth_Angle 

(Dc_P_Ne  :  Direction_Cosine_Matrix_Elements; 
Dc_P_Nn  :  Direction_Cosine_Matrix_Elements) 
return  tfander_Angles  is 


begin 

return  Arctan(Dc_P_Ne  /  Dc_P_Nn ) ; 
end  Compute_Wander_Azimuth_Angle; 
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separate  (Wander_Azimuth_Navigation_Parts) 
function  Compute_East_Veloci ty_Wi th_Sin_Cos_In 

(Nominal_East_Velocity  :  Velocities; 

Nominal_North_Velocity  :  Velocities; 

Sine_Of_tfander_Angle  :  Sin_Cos_Ratio; 

Cosine_Of_Vander_Angle  :  Sin_Cos_Ratio)  return  Velocities  is 


—  — declaration  of  variables- 


Answer  :  Velocities; 


— begin  Junction  Compute  East  Velocity 


begin 

Answer  :=  Nominal_East_Velocity  *  Cosine_Of_Wander_Angle  - 
Nominal_North_Velocity  *  Sine_Of_Wander_Angle; 

return  Answer; 

end  Compute_Eas t_Veloci ty_Wi th_Sin_Cos_In ; 


.  .  .v.v.v.'v. 
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separate  (Wander_Azimuth_Navigation_Parts) 
function  Compute_North_Velocity_Wi th_Sin_Cos_In 
(Nominal_East_Velocity  :  Velocities; 

Nominal_North_Velocity  :  Velocities; 

Sine_Of_Wander_Angle  :  Sin_Cos_Ratio; 

Cosine  Of  Wander  Angle  :  Sin_Cos_Ratio)  return  Velocities  is 


—  —  declaration  of  variables- 


Answer  :  Velocities; 


—  begin  function  Compute  North  Velocity 


begin 

Answer  :=  Nominal_East_Velocity  *  Sine_Of_Wander_Angle  + 
Nominal_North_Veloeity  *  Cosine_Of_Wander_Angle; 

return  Answer; 

end  Compute_Nor th_Veloci ty_Wi th_Sin_Cos_In; 
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separate  (Wander_Azimuth_Navigation_Parts) 

procedure  Compute_Earth_Relative_Horizontal_Velocities_With_Sin_Cos_In 
(Nominal_East_Velocity  :  in  Velocities; 

Nominal_North_Velocity  :  in  Velocities; 

Sine_Of_Wander_Angle  :  in  Sin_Cos_Ratio; 

Cosine_Of_Wander_Angle  :  in  Sin_Cos_Ratio; 

East_Velocity  :  out  Velocities; 

North_Velocity  :  out  Velocities)  is 

begin 

East_Velocity  :=  Nominal_East_Velocity  *  Cosine_Of_Wander_Angle  - 
Nominal_North_Velocity  *  Sine_Of_Vander_Angle; 
North_Velocity  :=  Norainal_East_Velocity  *  3ine  OfJ/ander  Angle  + 
Nominal_North_Velocity  *  CosIne_Of_Wan<Ier_Angle; 

end  Compute_Earth_Relative_Horizontal_Velocities_With_Sin_Cos_In; 
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separate  ( Wander_Azimu  th_Naviga  t ion_Par  t s ) 
function  Compute_Latitude_Using_Two_Value_Arctangent 

(Dc_P_V  :  Direction_Cosine_Matrix_Elements; 
Dc_P_Ne  :  Direction_Cosine_Matrix_Elements; 
Dc_P_Nn  :  Direction_Cosine_Matrix_Elements) 
return  Earth  Positions  is 


—  —  declaration  of  variables- 


Answer  :  Earth_Positions; 

Denominator  :  Direction  Cosine  Matrix  Elements; 


—  beginning  of  Junction- 


begin 

Denominator  :=  Sqrt(  Dc_P_Ne  *  Dc_P_Ne  + 

Dc_P_Nn  *  Dc_P_Nn  ); 

Answer  :=  Arctan2  (Y  =>  Dc_P_V, 

X  =»>  Denominator); 

return  Answer; 

end  Compu  t  e_La  t i t ude_Us i ng_Two_Value_Arc  t  angen  t ; 
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separate  (Wander_Azimuth_Navigation_Parts) 
function  Compute_Longi tude_Using_Two_Value_Arctangent 
(Dc_R_V  :  Direction_Cosine_Matrix_Elements; 
Dc_G_V  :  Direction_Cosine_Matrix_Elements) 
return  Earth  Positions  is 


—  —  declaration  section 


Answer  :  Earth  Positions; 


—  begin  function 


begin 

return  Arctan2  (Y  =>  Dc_R_V, 

X  =>  Dc_G_V); 

end  Compute_Longitude_Using_Two_Value_Arctangent ; 
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separate  (Wander_Azimuth_Navigation_Parts) 

function  Compute_Wander_Azimuth_Angle_Using_Two_Value_Arctangent 
(Dc_P_Ne  :  Direction_Cosine_Matrix_Elements; 

Dc_P_Nn  :  Direction_Cosine_Matrix_Elements) 
return  Wander_Angles  is 


—  — declaration  section 


Answer  :  Wander  Angles; 


—  begin  function 


begin 

Answer  :=  Arctan2  (Y  =>  Dc_P_Ne, 

X  =>  Dc_P_Nn) ; 

return  Answer; 

end  Compute_Wander_Azimuth_Angle_Using_Two_Value_Arctangent ; 
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3. 3. 2. 4  DIRECTION j:OSINEJiATRIX_OPERATIONS  (PACKAGE  BODY)  TLCSC  P644  (CATALOG 
#P286-0) 


This  part  is  the  body  of  an  Ada  package  that  contains  units  for  all  CAMP  parts 
that  initialize  or  update  a  direction  cosine  matrix  (DCM).  A  direction  cosine 
matrix  is  a  3x3  matrix  whose  elements  are  cosines  of  the  angles  between  two 
coordinate  frames  -  xyz  and  XYZ.  The  direction  cosines  are  used  to  transform 
data  values  represented  in  one  coordinate  frame  to  another  coordinate  frame. 

This  part  consists  of  two  packages.  The  first  package  contains  all  parts  which 
operate  on  a  "general"  direction  cosine  matrix.  A  "general"  DCM  is  a  DCM  that 
does  not  assume  particular  coordinate  frames,  i.e.  the  Navigation  to  Earth. 
However,  these  parts  do  assume  a  particular  coordinate  frame  axes  relationship 
(for  an  example,  see  the  preceding  paragraph).  The  user  should  refer  to  the 
documentation  for  each  part  to  determine  if  the  unit  can  be  used  for  a 
particular  application. 

The  second  package  contains  all  operations  that  operate  on  a  CNE  matrix.  The 
CNE  matrix  is  a  direction  cosine  matrix  that  is  used  to  transform  objects  from 
the  navigation  frame  to  the  earth  frame.  The  navigation  frame  assumes  "East, 
North,  and  Up"  (for  XYZ),  and  the  earth  frame  assumes  "Greenwich,  Right,  and 
Polar"  (for  xyz)  for  the  axes  definitions.  These  coordinate  assumptions  are 
reflected  in  the  generic  objects  of  the  package.  This  package  exports  a  CNE 
matrix  type  representing  a  CNE  matrix  derived  from  these  coordinate 
assumptions. 

The  decomposition  for  this  part  is  the  same  as  that  shown  in  the  Top-Level 
Design  Document. 


3. 3. 2. 4.1  REQUIREMENTS  ALLOCATION 

The  following  chart  summarizes  the  allocation  of  CAMP  requirements  to  this 
part: 


|  Name 


|  Requirements  Allocation 


|  DCM_General_Operations  |  R34 

j  CNE_Operations  j  R32 


jj '  H 

3. 3. 2. 4. 2 

LOCAL  ENTITIES 

None. 

3. 3. 2. 4. 3 

INPUT/OUTPUT 

None. 

•3 

V\ 
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3. 3. 2. 4. 4  LOCAL  DATA 
None. 

3. 3. 2. 4. 5  PROCESS  CONTROL 
Not  applicable. 

3. 3. 2. 4. 6  PROCESSING 

The  following  describes  the  processing  performed  by  this  part: 
package  body  Direction_Cosine_Matrix_Operations  is 
package  body  DCM_General_Operations  is  separate; 
package  body  CNE_Operations  is  separate; 
end  Direction_Cosine_Matrix_Operations ; 

3. 3. 2. 4. 7  UTILIZATION  OF  OTHER  ELEMENTS 
None. 

3. 3. 2. 4. 8  LIMITATIONS 
None. 

3. 3. 2. 4. 9  LLCSC  DESIGN 
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3. 3. 2. 4. 9.1  DCM_GENERAL_OPERATIONS  PACKAGE  DESIGN  (CATALOG  #P287-0) 

This  package  contains  procedures  and  functions  which  operate  on  a  "general" 
direction  cosine  matrix.  A  specific  coordinate  frame  axis  ordering  has  been 
assumed  for  the  parts;  therefore,  refer  to  the  documentation  for  each  part 
before  using  it. 

The  decomposition  for  this  part  is  the  same  as  that  shown  in  the  Top-Level 
Design  Document. 

3. 3. 2. 4. 9. 1.1  REQUIREMENTS  ALLOCATION 

The  following  table  summarizes  the  allocation  of  CAMP  requirements  to  this 
part: 


|  Name  |  Requirements  Allocation  | 


DCM  Initialized_From_Reference  |  N/A 
PerIorm_Trapezoidal_Integration_of_DCM  j  R34 
Perform_Rectangular_Integration_of_DCM  j  N/A 
Reorthonormalize_DCM  j  N/A 
Frame_Misalignment  |  N/A 
Aligned_DCM_Matrix  j  N/A 
Compute_First_Row_From_Orthonormal  j  N/A 
DCM  From  Quaternion  N/A 


3. 3. 2. 4. 9. 1.2  LOCAL  ENTITIES  DESIGN 
None. 


3. 3. 2. 4. 9. 1.3  INPUT/OUTPUT 
None. 


3. 3. 2. 4. 9. 1.4  LOCAL  DATA 
None. 


3. 3. 2. 4. 9. 1.5  PROCESS  CONTROL 
Not  applicable. 


3. 3. 2. 4. 9. 1.6  PROCESSING 

The  following  describes  the  processing  performed  by  this  part: 

separate  (Direction_Cosine_Matrix_Operations) 
package  body  DCM_General_Operations  is 


CAMP  Software  Detailed  Design  Document 


Page 


function  DCM_Initialized_From_Reference 

(Ref_DCM_2_l  :  Sin_Cos_Ratio; 

Ref_DCM_2_2  :  Sin_Cos_Ratio; 

Ref_DCM_3_l  :  Sin_Cos_Ratio; 

Ref_DCM_3_2  :  Sin_Cos_Ratio; 

Sign_0f_2_3  :  INTEGER; 

Sign_0f_3_3  :  INTEGER) 

return  Direction_Cosine_Matrix  is  separate; 

package  body  DCM_Trapezoidal_Integration  is  separate; 

procedure  ?erform_Rectangular_Integration_of_DCM 

(DC_Matrix  :  in  out  Direction_Cosine_Matrix; 

X_Rho  :  in  Angular_Velocities; 

Y_Rho  :  in  Angular_Velocities; 

Delta_Time  :  in  Time_Intervals)  is  separate; 

procedure  Reorthonormalize_DCM 

(DCH_Hatrix  :  in  out  Direction_Cosine_Matrix)  is  separate; 

function  Frame_Misalignment 

(DCM_Matrix  :  Direction_Cosine_Matrix; 

Ref_DC_Matrix  :  Direction_Cosine_Matrix) 
return  Rotation_Angle_Vec  is  separate; 

function  Aligned_DCM_Matrix 

(DCM_Matrix  :  Direction_Cosine_Matrix; 

Rotation_Angle  :  Rotation_Angle_Vec) 
return  Direction_Cosine_Matrix  is  separate; 

procedure  Compute_First_Rov_From  Orthonormal 

(DCM_Hatrix  ~  :  In  out  Direction_Cosine_Matrix)  is  separate 

function  DCM_From_Quaternion 

(Quaternion  :  Quaternion_Vectors) 
return  Direction  Cosine  Matrix  is  separate; 

end  DCM  General  Operations; 


3. 3. 2. 4. 9. 1.7  UTILIZATION  OF  OTHER  ELEMENTS 
None. 


3. 3. 2. 4. 9. 1.8  LIMITATIONS 
None. 


3. 3. 2. 4. 9. 1.9  LLCSC  DESIGN 
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3. 3. 2. 4. 9. 1.9.1  DCM_TRAPEZOIDAL_INTEGRATION  PACKAGE  DESIGN  (CATALOG  #P289-0) 

This  package  contains  a  procedure  that  integrates  a  direction  cosine  matrix 
using  trapezoidal  integration.  NOTE:  Only  rows  2  and  3  are  integrated  by  this 
part.  Row  1  is  usually  updated  from  rows  2  and  3  -  the  user  must  update  row  1 
manually. 

The  decomposition  for  this  part  is  the  same  as  that  shown  in  the  Top-Level 
Design  Document. 


3. 3. 2. 4. 9. 1.9. 1.1  REQUIREMENTS  ALLOCATION 

The  following  table  summarizes  the  allocation  of  CAMP  requirements  to  this 
part: 


|  Name 

|  Requirements  Allocation  | 

|  Perform  Trapezoidal_Integration_of_DCM 

1  R34  | 

3. 3. 2. 4. 9. 1.9. 1.2  LOCAL  ENTITIES  DESIGN 
None. 


3. 3. 2. 4. 9. 1.9. 1.3  INPUT/OUTPUT 
GENERIC  PARAMETERS: 

Data  types: 

The  following  table  describes  the  generic  formal  types  required  by  this  part: 


I 

I 


I 
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Name 

1  Type 

Angular 

_Velocities 

|  floating 
j  point 

1  type 

Time 

Intervals 

j  floating 
|  point 

1  type 

Row_Indices 

j  discrete 

1  type 

Col_Indices 

j  discrete 

S  type 

Sin_Cos_Ratio 

j  floating 
|  point 

Direction 

Cosine 

Matrix 

|  3X3 

<  matrix 

j  Description 


!  This  type  translates  into  the  type  of  the 
angular  velocity  used  to  integrate 
the  direction  cosine  matrix. 

This  type  translates  into  the  type  of  the 
time  parameter  used  to  integrate  the 
direction  cosine  matrix. 

Data  type  which  translates  into  the 

row  indexing  of  the  direction  cosine  matrix. 

Data  type  which  translates  into  the 

column  indexing  of  the  direction  cosine 
matrix. 

Type  which  translates  into  the  type  of  the 
elements  of  the  direction  cosine  matrix. 

Type  representing  a  direction  cosine  matrix 
indexed  by  the  column  and  row  types  described 
above  with  elements  of  type  Sin  Cos  Ratio. 


Data  objects: 

The  following  table  describes  the  generic  formal  objects  required  by  this  part 
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|  Name 

Type 

Value 

Description  | 

| Initial 
|  X  Rho 

j  ‘  _ 

Angular 

_Velocities 

0 

or 

user- 

defined 

Object  used  to  set  the  initial  angular 
velocity  X  Rho  to  either  0  (by  default) 
or  to  a  user-specified  value.  Note  that 
this  value  is  set  at  compile  time.  To  set 
the  value  at  run  time,  use  the  reinitial¬ 
ize  procedure. 

| Initial 
|  _Y_Rho 

Angular 

Velocities 

0 

or 

user- 

defined 

Object  used  to  set  the  initial  angular 
velocity  Y  Rho  to  either  0  (by  default) 
or  to  a  user-specified  value.  Note  that 
this  value  is  set  at  compile  time.  To  set 
the  value  at  run  time,  use  the  reinitial¬ 
ize  procedure. 

|  Rowl 

Row  Indices 

' FIRST 

Object  to  be  used  as  an  index  representing 
the  first  row  of  the  direction  cosine 
matrix.  This  object  indexes  the  direction 
cosines  of  the  angles  between  the  x-axis 
and  the  XYZ  axes. 

! 

j  Row2 

Row_Indices 

SUCC 

'FIRST 

Object  to  be  used  as  an  index  representing 
the  second  row  of  the  direction  cosine 
matrix.  This  object  indexes  the  direction 
cosines  of  the  angles  between  the  y-axis 
and  the  XYZ  axes. 

|  Row3 

1 

Row_Indices 

'LAST 

Object  to  be  used  as  an  index  representing 
the  third  row  of  the  direction  cosine 
matrix.  This  object  indexes  the  direction 
cosines  of  the  angles  between  the  z-axis 
and  the  XYZ  axes. 

|  Coll 

I 

Col_Indices 

'FIRST 

Object  to  be  used  as  an  index  representing 
the  first  column  of  the  direction  cosine 
matrix.  This  object  indexes  the  direction 
cosines  of  the  angles  between  the  X-axis 
and  the  xyz  axes. 

|  Col2 

1 

! 

1 

Col_Indices 

SUCC 

'FIRST 

Object  to  be  used  as  an  index  representing 
the  second  column  of  the  direction  cosine 
matrix.  This  object  indexes  the  direction 
cosines  of  the  angles  between  the  Y-axis 
and  the  xyz  axes. 

j  Col3 

1 

1 

1 

Col_Indices 

'LAST 

Object  to  be  used  as  an  index  representing 
the  third  column  of  the  direction  cosine 
matrix.  This  object  indexes  the  direction 
cosines  of  the  angles  between  the  Z-axis 
and  the  xyz  axes. 

Subprograms : 


JVr_ V-WH- -A 
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The  following  table  describes  the  generic  formal  subroutines  required  by  this 
part : 


|  Name  |  Type  |  Description 


I  function 


function 


This  function  multiplies  an  object  of  type 

Angular_Velocities  to  an  object  of  type  Sin_Cos_Ratio 
returning  an  object  of  type  Angular_Velocities. 

This  function  multiplies  an  object  of  type 
Angular_Velocities  to  an  object  of  type 
Time_Intervals  returning  an  object  of  type 
Sin  Cos  Ratio. 


3. 3. 2. 4. 9. 1.9. 1.4  LOCAL  DATA 


Data  objects: 

The  following  table  describes  the  data  objects  created  and  used  by  this  part: 


Name  j  Type 


|  Description 


|Prev 

1 

Angular  | 

This  value  is  the  previous  X  angular  velocity  used  as 

1 

1  x 

| 

Velocities  j 

the  lower  bound  of  the  integration. 

1 

j  Rho 

i 

I 

1 

1 

|Prev 

1 

1 

1 

Angular 

This  value  is  the  previous  Y  angular  velocity  used  as 

1 

1 

j _ Y 

1 

Velocities  j 

the  lower  bound  of  the  integration. 

1 

j  Rho 

1 

1 

Exceptions: 


3. 3. 2. 4. 9. 1.9. 1.5  PROCESS  CONTROL 
Not  applicable. 


3. 3. 2. 4. 9. 1.9. 1.6  PROCESSING 

The  following  describes  the  processing  performed  by  this  part: 

separate  ( Direction_Cosine_Matrix_Operat ions .DCM_General_Operat ions) 
package  body  DCM_Trapezoidal_Integration  is 

Prev_X  Rho  :  Angular_Velocities  :=  Ini tial_X_Rho; 

Prev_Y~Rho  :  Angular_Velocities  :=  Ini tial_Y_Rho; 

procedure  Reini tialize_Angular_Velocities 

(X_Rho~  :  in  Angular_Velocities; 

Y_Rho  :  in  Angular_Veloci ties)  is  separate; 
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procedure  Perform_Trapezoidal_Integration_of_DCM 

(DC  Matrix  :  in  out  Direction  Cosine  Matrix; 


X  Rho 

:  in 

Angular  Velocities; 

Y_Rho 

:  in 

Angular_Veloci t ies  5 

Delta  Time 

:  in 

Time  Intervals)  is  separate; 

end  DCM_Trapezoidal_Integration; 

3. 3. 2. 4. 9. 1.9. 1.7  UTILIZATION  OF  OTHER  ELEMENTS 
None. 

3. 3. 2. 4. 9. 1.9. 1.8  LIMITATIONS 
None. 

3. 3. 2. 4. 9. 1.9. 1.9  LLCSC  DESIGN 
None. 

3.3.2.4.9.1.9.1:10  UNIT  DESIGN 

3.3.2.4.9.1.9.1.10.1  REINITIALIZE_ANGULAR_VELOCITIES  UNIT  DESIGN 

This  unit  reinitalizes  the  angular  velocities  which  are  used  as  the  previous 
angular  velocities  in  Trapezoidal  Integration. 

3.3.2.4.9.1.9.1.10.1.1  REQUIREMENTS  ALLOCATION 
N/A 

3.3.2.4.9.1.9.1.10.1.2  LOCAL  ENTITIES  DESIGN 
None. 

3.3.2.4.9.1.9.1.10.1.3  INPUT/OUTPUT 
GENERIC  PARAMETERS: 

Data  types: 

The  following  table  summarizes  the  generic  formal  types  required  by  this  part: 


|  Name 

Type 

Description  | 

|  Angular 
j  Velocities 

1  " 

floating 

point 

type 

This  type  translates  into  the  type  of  the  | 

angular  velocity  used  to  integrate  j 

the  direction  cosine  matrix.  j 
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FORMAL  PARAMETERS: 

The  following  table  describes  this  part's  formal  parameters: 


|  Name  |  Type  |  Mode  |  Description 


X  Rho 


Angular 

Velocities 


In 


Y  Rho 


Angular 

Velocities 


In 


This  object  is  the  X  angular  velocity  that 
will  be  used  to  reinitialize  the  previous 
X  angular  velocity. 

This  object  is  the  Y  angular  velocity  that 
will  be  used  to  reinitialize  the  previous 
Y  angular  velocity. 


3.3.2.4.9.1.9.1.10.1.4  LOCAL  DATA 


None. 


3.3.2.4.9.1.9.1.10.1.5  PROCESS  CONTROL 
Not  applicable. 


3.3.2.4.9.1.9.1.10.1.6  PROCESSING 

The  following  describes  the  processing  performed  by  this  part: 

separate  (Direction_Cosine_Matrix_Operations.DCM_General_Operations. 

DCM_Trapezoidal_Integration) 
procedure  Reinitialize_Angular_Velocities 

(X_Rho  :  in  Angular_Velocities; 

Y_Rho  :  m  Angular_Velocities)  is 

begin 

Prev_X_Rho  :=  X_Rho; 

Prev_Y_Rho  : =  Y_Rho ; 

end  Reini t ialize_Angular_Veloci t ies ; 


3.3.2.4.9.1.9.1.10.1.7  UTILIZATION  OF  OTHER  ELEMENTS 
UTILIZATION  OF  EXTERNAL  ELEMENTS: 

UTILIZATION  OF  OTHER  ELEMENTS  IN  TOP  LEVEL  COMPONENT: 

The  following  tables  describe  the  elements  used  by  this  part  but  defined 
elsewhere  in  the  parent  top  level  component: 

Data  objects: 


*>*-*_*',  J*_*L*V  »  >  *  *  -#  J*  >  M*  «  *  J.  ^  4%  .V*  *  Jm  *  4.%.  .  *1  _ 


3 

'i 

3! 


3 

"2 


3 
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The  following  table  summarizes  the  objects  required  by  this  part  and  defined 
elsewhere  in  the  parent  top  level  component: 


|  Name  |  Type  |  Source  |  Description 


Prev_X 

Rho 


Angular 

Velocities 


Direction 

_Cosine 

_Matrix 

_Operations. 

DCM_General 

_Operations. 

DCM 

_Trapezoidal 

_Integration 


This  object  is  the  angular  velocity 
in  the  X  direction  that  is  used 
as  the  previous  angular  velocity 
for  Trapezoidal  integration. 


Prev_Y 

Rho 


Angular 

Velocities 


Direction  | 
_Cosine  j 
_Matrix  [ 
_Operations. | 
DCM_General  | 
_Operations. j 
DCM  | 
JTrapezoidal | 
_Integrationj 


This  object  is  the  angular  velocity 
in  the  Y  direction  that  is  used 
as  the  previous  angular  velocity 
for  Trapezoidal  integration. 


3.3.2.4.9.1.9.1.10.1.8  LIMITATIONS 


None. 


3.3.2.4.9.1.9.1.10.2  PERFORM_TRAPEZOIDAL_INTEGRATION_OF_DCM  UNIT  DESIGN 

This  part  integrates  a  direction  cosine  matrix  using  trapezoidal  integration. 
Only  the  2nd  and  3rd  rows  are  integrated.  The  first  row  must  be  computed  by 
another  function. 


3.3.2.4.9.1.9.1.10.2.1  REQUIREMENTS  ALLOCATION 
N/A 

3.3.2.4.9.1.9.1.10.2.2  LOCAL  ENTITIES  DESIGN 
None. 


3.3.2.4.9.1.9.1.10.2.3  INPUT/OUTPUT 


GENERIC  PARAMETERS: 
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The  following  table  summarizes  the  generic  formal  types  required  by  this  part: 


|  Name 


|  Description 


Angular 

Velocities 


Time 

Intervals 


Row  Indices 


Col  Indices 


Sin  Cos  Ratio 


Direction 

_Cosine 

Matrix 


floating 

point 

type 

floating 

point 

type 

discrete 

type 

discrete 

type 


floating 

point 

3X3 

matrix 


This  type  translates  into  the  type  of  the 
angular  velocity  used  to  integrate 
the  direction  cosine  matrix. 

This  type  translates  into  the  type  of  the 
time  parameter  used  to  integrate  the 
direction  cosine  matrix. 

Data  type  which  translates  into  the 

row  indexing  of  the  direction  cosine  matrix. 

Data  type  which  translates  into  the 

column  indexing  of  the  direction  cosine 
matrix. 

Type  which  translates  into  the  type  of  the 
elements  of  the  direction  cosine  matrix. 

Type  representing  a  direction  cosine  matrix 
indexed  by  the  column  and  row  types  described 
above  with  elements  of  type  Sin  Cos  Ratio. 


Data  objects: 

The  following  table  summarizes  the  generic  formal  objects  required  by  this 
part: 
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Name  |  Type 


|  Value  |  Description 


Initial 
X  Rho 


Initial 
Y  Rho 


Angular 

Velocities 


Angular 

Velocities 


Row  Indices 


Row  Indices 


Row  Indices 


Col  Indices 


Col  Indices 


Col  Indices 


user- 

defined 


user- 

defined 


'FIRST 


succ 

'FIRST 


'  FIRST 


SUCC 

'FIRST 


Object  used  to  set  the  initial  angular 
velocity  X_Rho  to  either  0  (by  default) 
or  to  a  user-specified  value.  Note  that 
this  value  is  set  at  compile  time.  To  set 
the  value  at  run  time,  use  the  reinitial¬ 
ize  procedure. 

Object  used  to  set  the  initial  angular 
velocity  Y_Rho  to  either  0  (by  default) 
or  to  a  user-specified  value.  Note  that 
this  value  is  set  at  compile  time.  To  set 
the  value  at  run  time,  use  the  reinitial¬ 
ize  procedure. 

Object  to  be  used  as  an  index  representing 
the  first  row  of  the  direction  cosine 
matrix.  This  object  indexes  the  direction 
cosines  of  the  angles  between  the  x-axis 
and  the  XYZ  axes. 

Object  to  be  used  as  an  index  representing 
the  second  row  of. the  direction  cosine 
matrix.  This  object  indexes  the  direction 
cosines  of  the  angles  between  the  y-axis 
and  the  XYZ  axes. 

Object  to  be  used  as  an  index  representing 
the  third  row  of  the  direction  cosine 
matrix.  This  object  indexes  the  direction 
cosines  of  the  angles  between  the  z-axis 
and  the  XYZ  axes. 

Object  to  be  used  as  an  index  representing 
the  first  column  of  the  direction  cosine 
matrix.  This  object  indexes  the  direction 
cosines  of  the  angles  between  the  X-axis 
and  the  xyz  axes. 

Object  to  be  used  as  an  index  representing 
the  second  column  of  the  direction  cosine 
matrix.  This  object  indexes  the  direction 
cosines  of  the  angles  between  the  Y-axis 
and  the  xyz  axes. 

Object  to  be  used  as  an  index  representing 
the  third  column  of  the  direction  cosine 
matrix.  This  object  indexes  the  direction 
cosines  of  the  angles  between  the  Z-axis 
and  the  xyz  axes. 


Subprograms: 
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The  following  table  summarizes  the  generic  formal  subroutines  required  by  this 
part: 


Name  |  Type  |  Description 


This  function  multiplies  an  object  of  type 

Angular_Velocities  to  an  object  of  type  Sin_Cos_Ratio 
returning  an  object  of  type  Angular  Velocities. 


This  function  multiplies  an  object  of  type 
Angular_Velocities  to  an  object  of  type 
Time_Intervals  returning  an  object  of  type 
Sin  Cos  Ratio. 


FORMAL  PARAMETERS: 

The  following  table  describes  this  part's  formal  parameters: 


|  Description 


This  object  is  the  direction  cosine  matrix  to  be  integrated. 

This  object  is  the  new  X  angular  velocity  that  is  used  as  the 
upper  bound  in  the  integration. 

This  object  is  the  new  Y  angular  velocity  that  is  used  as  the 
upper  bound  in  the  integration. 

This  object  is  the  time  increment  since  the  last  angular 
velocity  update. 


3.3.2.4.9.1.9.1.10.2.4  LOCAL  DATA 
Data  objects: 

The  following  table  describes  the  data  objects  maintained  by  this  part: 
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I  Type 


|Value|  Description 


Prev  DCM 


X  Bound  Sum 


Y  Bound  Sum 


Del  T  div  2 


Direction 

_Cosine 

_Matrix 

Angular 

Velocities 


Angular 

Velocities 


Time 

Intervals 


This  object  holds  the  incoming  DCM 
matrix  for  use  in  the  integration. 


This  object  is  the  sura  of  the  lower  and 
upper  bounds  (angular  velocity  in  X 
direction)  used  in  the  integration. 

This  object  is  the  sum  of  the  lower  and 
upper  bounds  (angular  velocity  in  Y 
direction)  used  in  the  integration. 

This  object  is  the  time  interval 
between  the  previous  and  current 
angular  velocities  -  divided  by  2 
(for  use  in  the  integration). 


3.3.2.4.9.1.9.1.10.2.5  PROCESS  CONTROL 
Not  applicable. 


3.3.2.4.9.1.9.1.10.2.6  PROCESSING 

The  following  describes  the  processing  performed  by  this  part: 

separate  (Direction_Cosine_Matrix_Operations.DCM_General_Operations. 

DCM  Trapezoidal_Integration) 
procedure  Perform_Trapezoidal_Integration_of_DCM 

(DCMatrix  :  in  out  Direction_Cosine_Matrix; 


X  Rho 

:  in 

Angular_Velocities; 

Y_Rho 

:  in 

Angular_Velocities; 

Delta_Time 

:  in 

Tirae_Intervals)  is 

—  — <objects  for  internal  calculations> — 


Prev_DCM  :  Direction_Cosine_Matrix; 
X_Bound_Sum  :  Angular_Velocities; 
Y_Bound_Sum  :  Angular_Velocities; 

Del  T  div  2  :  Time  Intervals; 


— begin  procedure 


begin 

—  - <copy  last  two  rows  of  DCM  into  previous  DCM  before  computing^ 

Prev_DCM(Row2,Coll)  :=  DC_Matrix(Row2,Coll) ; 


CAMP  Software  Detailed  Design  Document 


Prev_DCM(Row2,Col2)  :=  DC_Matrix(Row2,Col2) ; 

Prev_DCM(Row2,Col3)  :=  DC_Matrix(Row2,Col3) ; 

Prev_DCM(Row3,Coll)  :=  DC_Matrix(Row3,Coll) ; 

Prev_DCM(Row3,Col2)  :=  DC_Matrix(Row3,Col2) ; 

Prev_DCM(Row3,Col3)  :=  DC_Matrix(Row3,Col3) ; 

—  — <compute  sum  of  X  and  Y  bounds  from  old  and  new  angular  velocity>- 

X_Bound_Sum  :=  (X_Rho  +  Prev_X_Rho); 

Y_Bound_Sum  :=  (Y_Rho  +  Prev_Y_Rho) ; 

—  — <compute  delta  time  divided  by  2  -  for  algorithm  simplification> — 
Del_T_div_2  :=  Delta_Time  *  Time_Intervals(0.5); 

- <compute  new  DCM> - 

DC_Matrix(row3,coll)  :  =  Prev_DCM(row3,coll)  - 

(  Y_Bound_Sum  *  Prev_DCM(row3,col3) 

*  Del_T_div_2) ; 

DC_Matrix(row3,col2)  :=  Prev_DCM{row3,col2)  + 

(  X__Bound_Sum  *  Prev_DCM(rov3,col3) 

*  Del_T_div_2) ; 

DC_Matrix(row3,col3)  :=  Prev_DCM(rov3,col3)  + 

(  Y_Bound_Sum  *  Prev_DCM( rov3 , coll )  - 
X_Bound  Sum  *  Prev_DCM(rov3,col2)  ) 

*  Del_T_dIv_2j 

DC_Matrix(row2,coll)  :=  Prev_DCM(rov2,coll)  - 

(  Y_Bound_5um  *  Prev_DCM(row2,col3) 

*  Del_T_div_2) j 

DC_Matrix(row2,col2)  :=  Prev_DCM(row2, col2)  + 

(  X_Bound_Sum  *  Prev_DCM(row2,col3) 

*  Del_T_div_2) ; 

DC_Matrix(row2,col3)  :=  Prev_DCM(row2,col3)  + 

(  Y_Bound_Sum  *  Prev_DCM(row2,coll)  - 
X_Bound  Sum  *  Prev_DCM(rov2,col2)  ) 

*  Del_T_div_2 ; 

Prev_XJRho  :=  X_Rho ; 

Prav_Y_Rho  : =  Y_Rho ; 

end  Perform_Trapezoidal_Integration_of_DCM; 


3.3.2.4.9.1.9.1.10.2.7  UTILIZATION  OF  OTHER  ELEMENTS 

UTILIZATION  OF  OTHER  ELEMENTS  IN  TOP  LEVEL  COMPONENT: 

The  following  tables  describe  the  elements  used  by  this  part  but  defined 
elsewhere  in  the  parent  top  level  component: 


-  .Y«  A  A  ^  JV  «V  ,  X  ,V  ,  *  “  ■ 
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Data  objects: 

The  following  table  summarizes  the  objects  required  by  this  part  and  defined 
elsewhere  in  the  parent  top  level  component: 


|  Name  |  Type  |  Source  |  Description 


Prev_X 

Rho 


Angular 

Velocities 


Direction 

_Cosine 

_Matrix 

_Operations. 

DCM_General 

_Operations. 

DCM 

_Trapezoidal 

_Integration 


This  object  is  the  angular  velocity 
in  the  X  direction  that  is  used 
as  the  previous  angular  velocity 
for  Trapezoidal  integration. 


Prev_Y 

Rho 


Angular 

Velocities 


Direction 

_Cosine 

_Matrix 

_Operations. 

DCM_General 

_Operations. 

DCM 

_Trapezoidal 

Integration 


This  object  is  the  angular  velocity 
in  the  Y  direction  that  is  used 
as  the  previous  angular  velocity 
for  Trapezoidal  integration. 


3.3.2.4.9.1.9.1.10.2.8  LIMITATIONS 


None. 


3.3.2.4.9.1.10  UNIT  DESIGN 

3.3.2.4.9.1.10.1  DCM  INITIALIZED  FROM  REFERENCE  UNIT  DESIGN  (CATALOG  #P288-0) 


This  function  initializes  a  direction  cosine  matrix  to  a  reference  direction 
cosine  matrix.  This  part  uses  the  orthonormal  properties  of  the  DCM  to  compute 
all  nine  elements  of  the  matrix  from  four  elements  of  the  reference  DCM  plus 
the  signs  from  two  other  reference  DCM  elements. 


This  implementation  assumes  that  these  four  elements  are  (y,X),  (y,Y),  (z,X), 
(z,Y),  where  xyz  and  .IYZ  are  the  two  coordinate  frames.  The  signs  are  computed 
from  elements  (y,Z)  and  (z,Z). 


3.3.2.4.9.1.10.1.1  REQUIREMENTS  ALLOCATION 
N/A 


.  “V 

A/vV' 
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3.3.2.4.9.1.10.1.2  LOCAL  ENTITIES  DESIGN 
None. 


3.3.2.4.9.1.10.1.3  INPUT/OUTPUT 
GENERIC  PARAMETERS: 

The  following  table  describes  the  generic  formal  types  required  by  this  part: 


Name  | 

Type 

Row  Indices  | 

i 

discrete 

type 

Col_Indices  j 

discrete 

type 

1 

Sin_Cos_Ratio| 

floating 

point 

Direction  j 

_Cosine  j 

"Matrix  j 

3X3 

matrix 

|  Description 


I 


ita  type  which  translates  into  the  j 

row  indexing  of  the  direction  cosine  matrix,  j 


Data  type  which  translates  into  the 

column  indexing  of  the  direction  cosine 
matrix. 


Type  which  translates  into  the  type  of  the 
elements  of  the  direction  cosine  matrix. 


Type  representing  a  direction  cosine  matrix 
indexed  by  the  column  and  row  types  described | 
above  with  elements  of  type  Sin_Cos_Ratio. 


Data  objects: 

The  following  table  describes  the  generic  formal  objects  required  by  this  part: 


-X  \X  3 

yqw  s 


Hi 
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|  Name 

Type 

Value 

Description  | 

|  Rowl 

1 

1 

1 

! 

Row_Indices 

' FIRST 

Object  to  be  used  as  an  index  representing 
the  first  row  of  the  direction  cosine 
matrix.  This  object  indexes  the  direction 
cosines  of  the  angles  between  the  x-axis 
and  the  XYZ  axes. 

|  Rov2 

i 

i 

i 

i 

Row_Indices 

SUCC 

'FIRST 

Object  to  be  used  as  an  index  representing 
the  second  row  of  the  direction  cosine 
matrix.  This  object  indexes  the  direction 
cosines  of  the  angles  between  the  y-axis 
and  the  XYZ  axes. 

i 

j  Row3 

1 

1 

1 

i 

Row_Indices 

'LAST 

Object  to  be  used  as  an  index  representing 
the  third  row  of  the  direction  cosine 
matrix.  This  object  indexes  the  direction 
cosines  of  the  angles  between  the  z-axis 
and  the  XYZ  axes. 

|  Coll 

i 

i 

i 

i 

Col_Indices 

'FIRST 

Object  to  be  used  as  an  index  representing 
the  first  column  of  the  direction  cosine 
matrix.  This  object  indexes  the  direction 
cosines  of  the  angles  between  the  X-axis 
and  the  xyz  axes. 

|  Col2 

1 

1 

i 

1 

Col_Indices 

SUCC 

'FIRST 

Object  to  be  used  as  an  index  representing 
the  second  column  of  the  direction  cosine 
matrix.  This  object  indexes  the  direction 
cosines  of  the  angles  between  the  Y-axis 
and  the  xyz  axes. 

|  Col3 

1 

1 

1 

1 

Col_Indices 

'LAST 

Object  to  be  used  as  an  index  representing 
the  third  column  of  the  direction  cosine 
matrix.  This  object  indexes  the  direction 
cosines  of  the  angles  between  the  Z-axis 
and  the  xyz  axes. 

Subprograms : 

The  following  table  describes  the  generic  formal  subroutines  required  by  this 
part: 


|  Name 

1  Type 

|  Description  | 

|  SORT 

1 

|  function 

1 

|  This  function  computes  the  square  root  of  a  value  of  the| 
j  type  Sin_Cos_Ratio.  j 

FORMAL 

PARAMETERS: 

The  following  table  describes  this  part's  formal  parameters: 
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|  Name 

Type 

Mode 

Description  | 

1  REF  DCM  2  1  1 

Sin  Cos  Ratio 

In 

This  value  is  the  reference  direction  | 

1  1 

1  1 

1  1 

1  1 

1  1 

cosine  element  located  at  row  2,  j 

column  1.  It  represents  the  j 

direction  cosine  of  the  angle  | 

between  the  y-axis  and  the  X-axis,  j 

• 

1 

1  REF  DCM  2  2 

Sin  Cos  Ratio 

In 

This  value  is  the  reference  direction  | 

1  1 

1  1 

1  i 

1  1 

1  1 

cosine  element  located  at  row  2,  j 
column  2.  It  represents  the  | 
direction  cosine  of  the  angle  j 
between  the  y-axis  and  the  Y-axis,  j 

i 

1 

1  REF  DCM  3  1 

Sin  Cos  Ratio 

In 

1 

This  value  is  the  reference  direction  | 

1  1 

1  1 

1  1 

1  1 

1  1 

cosine  element  located  at  row  3,  j 
column  1.  It  represents  the  | 
direction  cosine  of  the  angle  j 
between  the  z-axis  and  the  X-axis.  ‘ 

i 

1 

1  REF  DCM  3  2 

Sin  Cos  Ratio 

In 

1 

This  value  is  the  reference  direction  | 

i  i 

!  1 

i  t 

cosine  element  located  at  row  3,  j 
column  2.  It  represents  the  j 
direction  cosine  of  the  angle  j 
between  the  z-axis  and  the  Y-axis,  j 

i 

i 

|  Sign_0f_2_3 

1 

1 

1 

1 

1 

i 

1 

INTEGER 

In 

1 

This  value  represents  the  sign  of  the  | 
reference  direction  cosine  element  | 

located  at  row  2,  column  3.  This  \ 
value  is  the  sign  of  the  direction  j 
cosine  of  the  angle  between  the  J 

y-axis  and  the  Z-axis.  The  value  j 
must  equal  either  -1  or  1,  which  i 

represents  minus  or  plus,  1 

respectively.  \ 

1 

|  Sign  Of  3  3 

1  "  " 

1 

1 

INTEGER 

In 

This  value  represents  the  sign  of  the  | 
reference  direction  cosine  element  j 

located  at  row  3,  column  3.  This  j 
value  is  the  sign  of  the  direction  j 
cosine  of  the  angle  between  the  j 

z-axis  and  the  Z-axis.  The  value  j 
must  equal  either  -1  or  1,  which  j 

represents  minus  or  plus,  j 

respectively.  i 

I 


I 

a 


% 


4 


i 


1 


I 


3.3.2.4.9.1.10.1.4  LOCAL  DATA  | 

| 

Data  objects:  I 


The  following  table  describes  the  data  objects  maintained  by  this  part: 
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|  Name 

1  Type 

| Value  | 

Description  J 

|  DCM  Matrix  j 

|  Direction 

|  N.A.  | 

This  object  is  a  3X3  matrix  representing  | 

|  1 

|  Cosine 

1  i 

the  direction  consine  matrix  that  is  j 

I  1 

|  Matrix 

1  1 

computed  from  the  reference  DCM  which  j 

1  1 

1  1 

is  passed  in.  j 

3.3.2.4.9.1.10.1.5  PROCESS  CONTROL 
Not  applicable. 


3.3.2.4.9.1.10.1.6  PROCESSING 

The  following  describes  the  processing  performed  by  this  part: 


separate  (Direction_Cosine_Matrix_Operations.DCM_General_Operations) 
function  DCM  Initialized  From  Reference 


(REF_DCM_2_1 
REF_DCM_2_2 
REF_DCM_3_1 
REF_DCM_3_2 
Sign_0f_2_3 
Sign_0f  3_3 


Sin_Cos_Ratio; 

Sin_Cos_Ratio; 

Sin_Cos_Ratio; 

Sin~Cos_Ratio; 

INTEGER; 

INTEGER) 


return  Dlrection_Cosine_Matrix  is 

- direction  cosine  matrix  to  be  computed> — 

DCM_Matrix  :  Direction_Cosine_Matrix; 
begin 

- <compute  new  DCM> — 

if  Sign  Of  2  3  =>  1  then 


DCM  Matrix(Row2,Col3)  :=  SORT  (1.0  -  (  REF_DCM_2_1  * 

REF  DCM  2  1 


REF_DCM_2_2  * 
REF  DCM  2  2  ) 


DCM  Matrix(Row2,Col3)  :=  -  SORT  (1.0  -  (  REF_DCM_2_1  * 

REF  DCM  2  1 


REF_.DCM_2_2  * 
REF  DCM  2  2  ) 


end  if; 

if  Sign  Of  3  3  =  1  then 


?  t 
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DCM_Matrix(Rov3,Col3)  :=  SORT  (1.0  -  (  REF_DCM_3_1  * 

REF  DCM  3  1 


REF_DCM_3_2  * 
REF  DCM  3  2  ) 


DCM_Matrix(Rov3,Col3)  :=  -  SORT  (1.0  -  (  REF_DCM_3_1  * 

REF  DCM  3  1 


REF_DCM_3_2  * 
REF  DCM  3  2  ) 


end  if; 


DCM  Matrix(Rovl,Coll)  := 


REF_DCM_2_2  * 

DCM  Matrix(Row3,Col3) 


DCM_Matrix(Row2,Col3)  * 
REF  DCM  3  2; 


DCM  Matrix(Rovl,Col2)  := 


DCM_Matrix(Row2 , Col3)  * 
REF  DCM  3  1 


REF_DCM  2_1  * 

DCM  Matrix(Rov3,Col3); 


DCM  Matrix(Rowl , Col3)  := 


REF_DCM_2_1  * 
REF“DCM"3“2 


REF_DCH_2_2  * 
REF  DCM  3  1; 


DCM_Matrix(Row2 , Coll ) 
DCM_Matrix(Row2 , Col2 ) 
DCM_Matrix(Row3 , Coll ) 
DCM  Matrix(Row3,Col2) 


=  REF_DCM_2_1 
=  REF_DCM_2_2 
=  REF_DCM_3_1 
=  REF  DCM  3  2 


return  DCM  Matrix; 


end  DCM  Initialized  From  Reference; 


3.3.2.4.9.1.10.1.7  UTILIZATION  OF  OTHER  ELEMENTS 


None. 


3.3.2.4.9.1.10.1.8  LIMITATIONS 


None. 


Ih'jSLw  'jV  A  .*•  .V.%  .Y.SA\v.%  a.sV.'*..v,s 
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3.3.2.4.9.1.10.2  PERFORM_RECTANGULAR_INTEGRATION_OF_DCM  UNIT  DESIGN  (CATALOG  #P290-0) 

This  procedure  integrates  a  direction  cosine  matrix  using  rectangular 
integration.  Only  rows  2  and  3  are  updated.  Row  1  must  be  updated  by  another 
function  (using  the  orthonormal  property). 


3.3.2.4.9.1.10.2.1  REQUIREMENTS  ALLOCATION 
N/A 


3.3.2.4.9.1.10.2.2  LOCAL  ENTITIES  DESIGN 


None. 


3.3.2.4.9.1.10.2.3  INPUT/OUTPUT 
GENERIC  PARAMETERS: 

Data  types: 

The  following  table  describes  the  generic  formal  types  required  by  this  part: 


|  Name 

Type 

Description  | 

|  Angular 

floating 

This  type  translates  into  the  type  of  the 

S  Velocities 

point 

angular  velocity  used  to  integrate 

type 

the  direction  cosine  matrix. 

|  Time 

floating 

This  type  translates  into  the  type  of  the 

j  Intervals 

point 

time  parameter  used  to  integrate  the 

type 

direction  cosine  matrix. 

1 

|  Row  Indices 

discrete 

Data  type  which  translates  into  the 

type 

row  indexing  of  the  direction  cosine  matrix. 

1 

|  Col  Indices 

discrete 

Data  type  which  translates  into  the 

type 

column  indexing  of  the  direction  cosine 

1 

1 

matrix. 

1 

|  Sin  Cos  Ratio 

floating 

Type  which  translates  into  the  type  of  the 

point 

elements  of  the  direction  cosine  matrix. 

1 

|  Direction 

3X3 

Type  representing  a  direction  cosine  matrix 

j  Cosine 

matrix 

indexed  by  the  column  and  row  types  described 

[  _Matrix 

above  with  elements  of  type  Sin_Cos_Ratio. 

CAMP  Software  Detailed  Design  Document 


Page  236 


|  Name  |  Type 


Value 


Description 


Row  Indices 


Row  Indices 


Row  Indices 


Col  Indices 


Col  Indices 


Col  Indices 


'  FIRST 


succ 

'FIRST 


'FIRST 


SUCC 

'FIRST 


Object  to  be  used  as  an  index  representing 
the  first  row  of  the  direction  cosine 
matrix.  This  object  indexes  the  direction 
cosines  of  the  angles  between  the  x-axis 
and  the  XYZ  axes. 

Object  to  be  used  as  an  index  representing 
the  second  row  of  the  direction  cosine 
matrix.  This  object  indexes  the  direction 
cosines  of  the  angles  between  the  y-axis 
and  the  XYZ  axes. 

Object  to  be  used  as  an  index  representing 
the  third  row  of  the  direction  cosine 
matrix.  This  object  indexes  the  direction 
cosines  of  the  angles  between  the  z-axis 
and  the  XYZ  axes. 

Object  to  be  used  as  an  index  representing 
the  first  column  of  the  direction  cosine 
matrix.  This  object  indexes  the  direction 
cosines  of  the  singles  between  the  X-axis 
and  the  xyz  axes. 

Object  to  be  used  as  an  index  representing 
the  second  column  of  the  direction  cosine 
matrix.  This  object  indexes  the  direction 
cosines  of  the  angles  between  the  Y-axis 
and  the  xyz  axes. 

Object  to  be  used  as  an  index  representing 
the  third  column  of  the  direction  cosine 
matrix.  This  object  indexes  the  direction 
cosines  of  the  angles  between  the  Z-axis 
and  the  xyz  axes. 


Subprograms : 

The  following  table  describes  the  generic  formal  subroutines  required  by  this 
part: 


|  Name 

1  Type 

|  Description 

1 

|  »*» 

1 

1 

|  function 

I 

1 

|  This  function  multiplies 
|  Velocities  to  an  object 
j  returning  an  object  of 

an  object  of  type  Angular  j 

of  type  Time  Intervals  j 

type  Sin_Cos_Ratio.  | 

FORMAL  PARAMETERS: 
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The  following  table  describes  this  part's  formal  parameters: 


Name 

Type 

|Mode 

DC  iatrix 

Direction 

1 

in 

Cosine 

1 

out 

_Matrix 

1 

1 

X  Rho 

Angular 

1 

1 

in 

1  Velocities! 
"  1 

Y  Rho 

Angular 

1 

in 

Velocities] 
"  1 

1 

Delta  Time!  Time 

1 

1 

in 

1  Intervals 

1 

Description 


This  is  the  direction  cosine  matrix  being 
integrated. 


This  value  represents  the  angular  velocity 
of  the  x-axis  with  respect  to  the  X-axis 
in  the  two  coordinate  frames  xyz  and  XYZ. 

This  value  represents  the  angular  velocity 
of  the  y-axis  with  respect  to  the  Y-axis 
in  the  two  coordinate  frames  xyz  and  XYZ. 

This  value  is  the  time  interval  over  which 
the  integration  is  to  be  performed. 


3.3.2.4.9.1.10.2.4  LOCAL  DATA 


Data  objects: 

The  following  table  describes  the  data  objects  maintained  by  this  part: 


Name 

|  Type 

|Value 

Description 

1 

DCM  Incr 

| Direction 

|  N.A. 

This  object  is  a  direction  cosine  matrix  that 

1 

|  Cosine 

1 

is  used  as  temporary  storage  for  computed 

1 

|  _Matrix 

1 

| 

increments. 

1 

Y  Rho 

1 

|  Sin  Cos 

1 

Y  Rho 

This  object  holds  the  angular  velocity  Y  Rho 

1 

1 

Time-3 

j  Ratio 

!  * 

times  the  time  increment. 

1 

Delta 

|  Delta 

1 

_Time 

1 

j_Time 

1 

X  Rho 

|  Sin  Cos 

1 

X  Rho 

This  object  holds  the  angular  velocity  X  Rho 

1 

1 

Times 

i  Ratio 

1  * 

times  the  time  increment. 

1 

Delta 

|  Delta 

_Time 

1 

|_Tirae 

1 

i 


3.3.2.4.9.1.10.2.5  PROCESS  CONTROL 
Not  applicable. 
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3.3.2.4.9.1.10.2.6  PROCESSING 

The  following  describes  the  processing  performed  by  this  part: 

separate  (Direction_Cosine_Matrix_Operations.DCM_General_Operations) 
procedure  Perform_Rectangular_Integration_of_DCM 

(DC_Matrix  :  in  out  Direction_Cosine_Matrix; 

X_Rho  :  in  Angular_Velocities; 

Y_Rho  :  in  Angular_Velocities; 

Delta  Time  :  in  Time  Intervals)  is 


—  — declaration  section 


- <DCM  for  computing  increments  (temporary)  > - 

DCM_Incr  :  Direction_Cosine_Matrix; 

—  — <  objects  for  internal  calculation  > — 

Y_Rho_Times  DeltaJTime  :  Sin_Cos_Ratio  :=  Y_Rho  *  Delta_Time; 
X  Rho  Times- Delta  Time  :  Sin  Cos  Ratio  :=  X  Rho  *  Delta  Time; 


— begin  procedure 


begin 

- <compute  increments  for  bottom  two  rovs> — 

DCM_Incr(Row2,Coll)  :=»  -DC_Matrix(Row2,Col3)  *  Y_Rho_Times_Delta_Time; 

DCM_Incr(Row2,Col2)  :=  DC_Matrix(Row2,Col3)  *  X_Pho_Times_Delta_Time; 

DCM_Incr(Row2,Col3)  :=  DC_Matrix(Row2,Coll)  *  Y_Rho_Times_Delta_Time  - 

DC_Matrix(Row2,Col2)  *  X_Rho_Times_Delta_Time; 
DCM_Incr(Row3,Coll)  :<=  -DC_Matrix(Row3,Col3)  *  Y_Rho_Times_Delta_Time; 

DCM_Incr(Row3,Col2)  :=  DC_Matrix(Row3,Col3)  *  X_Rho_Times_Delta_Time; 

DCM_Incr(Row3,Col3)  :=  DC_Matrix(Row3,Coll)  *  Y_Rho_Times_Delta_Time  - 

DC_Matrix(Row3,Col2)  *  X_Rho_Times_Delta_Time; 

—  — <compute  current  values  of  bottom  two  Rows> — 

DC_Matrix(Row2,Coll)  :=  DC_Matrix(Row2,Coll)  +  DCM_Incr(Row2,Coll) ; 

DC_Matrix(Row2,Col2)  :=  DC_Matrix(Row2,Col2)  +  DCM_Incr(Row2,Col2) ; 

DC_Matrix(Row2,Col3)  :=  DC_Matrix(Row2,Col3)  +  DCM_Incr(Row2,Col3) ; 

DC_Matrix(Row3,Coll)  :=  DC_Matrix(Row3,Coll)  +  DCM_Incr(Row3,Coll); 

DC_Matrix(Rov3,Col2)  :=  DC_Matrix(Row3,Col2)  +  DCM_Incr(Row3,Col2) ; 

DC_Matrix(Row3,Col3)  :=  DC_Matrix(Row3,Col3)  +  DCM_Incr(Row3,Col3) ; 

end  Perform_Rectangular_Integration_of_DCM; 
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3.3.2.4.9.1.10.2.7  UTILIZATION  OF  OTHER  ELEMENTS 


None. 


3.3.2.4.9.1.10.2.8  LIMITATIONS 
None. 


3.3.2.4.9.1.10.3  REORTHONORMALIZE_DCM  UNIT  DESIGN  (CATALOG  #P291-0) 

This  procedure  reorthonormalizes  (when  executed  repeatedly)  the  bottom  two  rows 
of  the  direction  cosine  matrix.  These  two  rows  represent  the  y  and  z  axes  in  a 
direction  cosine  matrix  that  transforms  data  from  coordinate  frame  xyz  to  frame 
XYZ.  Note  that  the  first  row  is  not  updated. 

This  procedure  will  not  reorthonormalize  the  DCM  after  only  one  iteration.  It 
must  be  executed  repeatedly  at  a  rate  based  on  the  precision  of  the  data,  the 
update  frequency  of  the  DCM,  and  other  considerations.  For  example,  in  a 
typical  application,  this  function  is  executed  once  per  minute. 


3.3.2.4.9.1.10.3.1  REQUIREMENTS  ALLOCATION 
N/A 


3.3.2.4.9.1.10.3.2  LOCAL  ENTITIES  DESIGN 
None. 

3.3.2.4.9.1.10.3.3  INPUT/OUTPUT 
GENERIC  PARAMETERS: 

Data  types: 

The  following  table  describes  the  generic  formal  types  required  by  this  part: 


,  / 


i 


,*  »  •  "  *  * ,«  »>*„«.  *»  ">  v  v  w  V  V  V  V  V  V  v  "j.  v  \»  "j-  V  "»» ■„*  V  V  V  V*  V  V  V  V  V  V  ’.*•  W  V  V  V  V  V  V  V  V  w r  WJ 
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Name  j 

Type 

Row  Indices  | 

1 

discrete 

type 

1 

Col  Indices  | 

1 

discrete 

type 

1 

Sin_Cos_Ratio| 

1 

floating 

point 

1 

Direction  j 

Cosine  \ 

Matrix  j 

3X3 

matrix 

Description 


Data  type  which  translates  into  the 

row  indexing  of  the  direction  cosine  matrix. 

Data  type  which  translates  into  the 

column  indexing  of  the  direction  cosine 
matrix. 

Type  which  translates  into  the  type  of  the 
elements  of  the  direction  cosine  matrix. 

Type  representing  a  direction  cosine  matrix 
indexed  by  the  column  and  row  types  described! 
above  with  elements  of  type  Sin  Cos  Ratio. 


Data  objects: 

The  following  table  describes  the  generic  formal  objects  required  by  this  part: 


CAMP  Software  Detailed  Design  Document 


Page  241 


|  Name  |  Type 


|  Value  |  Description 


Row  Indices 


Row  Indices 


Row  Indices 


Col  Indices 


Col  Indices 


Col  Indices 


'FIRST 


SUCC 
' FIRST 


'FIRST 


SUCC 
' FIRST 


Object  to  be  used  as  an  index  representing 
the  first  row  of  the  direction  cosine 
matrix.  This  object  indexes  the  direction 
cosines  of  the  angles  between  the  x-axis 
and  the  XYZ  axes. 


Object  to  be  used  as  an  index  representing 
the  second  row  of  the  direction  cosine 
matrix.  This  object  indexes  the  direction 
cosines  of  the  angles  between  the  y-axis 
and  the  XYZ  axes. 


Object  to  be  used  as  an  index  representing 
the  third  row  of  the  direction  cosine 
matrix.  This  object  indexes  the  direction 
cosines  of  the  angles  between  the  z-axis 
and  the  XYZ  axes. 


Object  to  be  used  as  an  index  representing 
the  first  column  of  the  direction  cosine 
matrix.  This  object  indexes  the  direction 
cosines  of  the  angles  between  the  X-axis 
and  the  xyz  axes. 


Object  to  be  used  as  an  index  representing 
the  second  column  of  the  direction  cosine 
matrix.  This  object  indexes  the  direction 
cosines  of  the  angles  between  the  Y-axis 
and  the  xyz  axes. 


Object  to  be  used  as  an  index  representing 
the  third  column  of  the  direction  cosine 
matrix.  This  object  indexes  the  direction 
cosines  of  the  angles  between  the  Z-axis 
and  the  xyz  axes. 


FORMAL  PARAMETERS: 


The  following  table  describes  this  part's  formal  parameters: 


|  Name  |  Type 

|  Mode 

|  Description  | 

|  DCM_Matrix  |  Direction 
j  |  Cosine 

j  j  “Matrix 

1  in 

1 

1 

|  This  parameter  is  the  direction  cosine  j 

|  matrix  to  be  reorthonormalized.  j 

1  1 
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!* 


1 

I 


3.3.2.4.9.1.10.3.4  LOCAL  DATA 


Data  objects: 

The  following  table  describes  the  data  objects  maintained  by  this  part: 


Name 

1 

Type 

Row2  Dot  Row2 

1 

Real 

Row3  Dot  Row3 

1 

1 

1 

Real 

Row2  dot  Row3 

1 

1 

Real 

Row2  Dot  Row3 

1 

Half 

1 

Sin  Cos 

Row2  Dot  Row3 

1 

Ratio 

|  Description 


This  object  is  used  to  store  the  dot  product 
of  row  2  and  row  2  of  the  DCM. 

This  object  is  used  to  store  the  dot  product 
of  row  3  and  row  3  of  the  DCM. 

This  object  is  used  to  store  the  dot 
product  of  row  2  and  row  3  of  the  DCM. 

This  object  is  used  to  store  half  the  dot 
product  of  row  2  and  row  3  of  the  DCM. 


3.3.2.4.9.1.10.3.5  PROCESS  CONTROL 
Not  applicable. 


3.3.2.4.9.1.10.3.6  PROCESSING 

The  following  describes  the  processing  performed  by  this  part: 

separate  (Direction_Cosine_Matrix_Operations.DCM_General_Operations) 
procedure  Reorthonormalize_DCM 

(DCM_Matrix  :  in  out  Direction_Cosine_Matrix)  is 


—  — declaration  section 


—  — <objects  for  storing  dot  products> — 


Row2_Dot_Row2 
Row3_Dot_Row3 
Row2_Dot_Row3 
Half  Row2  Dot  Row3 


Real; 

Real; 

Real; 

Sin  Cos  Ratio; 


— begin  procedure 


begin 

Row2_Dot_Row2  :=  DCM_Matrix(Row2,Coll)  *  DCM_Matrix(Row2,Coll)  + 
DCM_Matrix(Row2,Col2)  *  DCM_Matrix(Row2,Col2)  + 
DCM_Matrix(Rcw2,Col3)  *  DCM_Matrix(Row2,Col3) ; 


Row3_Dot_Row3  :=  DCM_Matrix(Row3,Coll)  *  DCM_Matrix(Row3,Coll)  + 
DCM_Matrix(Row3,Col2)  *  DCM_Matrix(Row3,Col2)  + 
DCM  Matrix(Row3,Col3)  *  DCM_Matrix(Row3,Col3) ; 


r  r-  * 
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—  — calculate  column  values 


DCM_Matrix(Row2,Coll)  :=  DCM_Matrix(Row2,Coll)  * 

(1.5  -  Real(0.5)  *  Row2_Dot_Row2) ; 

DCM_Matrix(Row3,Coll)  :=  DCM_Matrix(Row3,Coll)  * 

(1.5  -  Real(0.5)  *  Row3_Dot_Row3) ; 

DCM_Matrix(Row2,Col2)  :=  DCM  Matrix(Row2,Col2)  * 

(1.3  -  Real (0.5)  *  Rov2_Dot_Row2) ; 

DCM_Matrix(Row3,Col2)  :=  DCM_Matrix(Row3,Col2)  * 

(1.5  -  Real(0.5)  *  Row3_Dot_Row3) ; 

DCM_Matrix(Row2,Col3)  :=  DCM  Matrix(Row2,Col3)  * 

(1.3  -  Real(0.5)  *  Row2_Dot_Row2) ; 

DCM  Matrix(Row3,Col3)  :=  DCM_Matrix(Row3,Col3)  * 

(1.5  -  Real(0.5)  *  Row3_Dot_Row3); 


—  — <compute  Rows  2  thru  3> 


Row2_Dot_Row3  :=  DCM_Matrix(Row2,Coll)  *  DCM_Matrix(Row3,Coll)  + 
DCM_Matrix(Row2 , Col2)  *  DCM_Matrix(Row3,Col2)  + 
DCM_Matrix(Row2,Col3)  *  DCM_Matrix(Row3»Col3); 

Half_Row2_dot_Row3  ;  =  0.5  *  Row2_dot_Row3f 

DCM_Matrix(Row2,Coll)  :=  DCM  Matrix(Row2,Coll)  - 

(HaIf_Row2_Dot_Row3  *  DCM_Matrix(Row3,Coll)) ; 

DCM_Matrix(Row3,Coll)  :=  DCM  Matrix(Row3,Coll)  - 

(HaIf_Row2_Dot_Row3  *  DCM_Matrix(Row2,Coll)); 

DCM_Matrix(Row2,Col2)  :=  DCM  Matrix(Row2,Col2)  - 

( Half _Row2_Do t_Row3  *  DCM_Matrix(Row3,Col2)); 

DCM_Matrix(Row3,Col2)  :=  DCM  Matrix(Row3,Col2)  - 

(HaTf_Row2_Dot_Row3  *  DCM_Matrix(Row2,Col2)); 

DCM_Matrix(Row2,Col3)  :=  DCM  Matrix(Row2,Col3)  - 

(HaIf_Row2_Dot_Row3  *  DCM_Matrix(Row3,Col3)) ; 

DCM_Matrix(Row3,Col3)  :=  DCM  Matrix(Row3,Col3)  - 

(HaIf_Row2_Dot_Row3  *  DCM_Matrix(Row2,Col3)) ; 

end  Reorthonormalize  DCM; 
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3.3.2.4.9.1.10.3.7  UTILIZATION  OF  OTHER  ELEMENTS 


None . 


3.3.2.4.9.1.10.3.8  LIMITATIONS 


/S?\  J 

lA* 


None. 


3.3.2.4.9.1.10.4  FRAME_MI SALIGNMENT  UNIT  DESIGN  (CATALOG  #P292-0) 

This  unit  computes  the  rotation  of  the  computed  XYZ  frame  with  respect  to  a 
reference  XYZ  frame  given  the  direction  cosine  matrix  of  the  computed  XYZ  frame 
and  the  direction  cosine  matrix  of  the  reference  XYZ  frame.  Each  rotation 
angle  provided  by  this  part  is  approximated  as  the  sine  of  the  rotation  angle. 


3.3.2.4.9.1.10.4.1  REQUIREMENTS  ALLOCATION 
N/A 


3.3.2.4.9.1.10.4.2  LOCAL  ENTITIES  DESIGN 


None. 


3.3.2.4.9.1.10.4.3  INPUT/OUTPUT 
GENERIC  PARAMETERS: 

Data  types: 

The  following  table  describes  the  generic  formal  types  required  by  this  part: 


i 

A 


1 

a 


A 


$ 

3 


rs 


"jt  ">  "  »  "  J.  , 
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|  Name  |  Type  |  Description 


|  Row  Indices 

|  discrete 
i  type 

l 

Data  type  which  translates  into  the  | 

row  indexing  of  the  direction  cosine  matrix,  j 

|  Col  Indices 

1 

|  discrete 

1  type 

1 

| 

Data  type  which  translates  into  the  j 

column  indexing  of  the  direction  cosine  j 

matrix.  j 

j  Sin  Cos  Ratio 

1 

|  floating 
|  point 

i 

Type  which  translates  into  the  type  of  the  j 

elements  of  the  direction  cosine  matrix.  ] 

j  Direction 
|  Cosine 
[  Matrix 

|  3X3 

1  matrix 

1 

Type  representing  a  direction  cosine  matrix  j 

indexed  by  the  column  and  row  types  described! 
above  with  elements  of  type  Sin  Cos  Ratio.  j 

j  Rotation 
|  Indices 

|  discrete 
!  type 

1 

Data  type  which  translates  into  the  j 

indexing  elements  of  a  rotation  angle  vector. j 

j  Rotation 

S  Angles 

|  floating 

|  point 

[ 

Type  which  translates  into  the  type  of  the  | 

elements  of  a  rotation  angle  vector.  j 

j  Rotation 
j  _Angle 
j  Vec 

|  1X3  matrix 
j  (vector) 

I 

Type  representing  a  rotation  angle  vector.  j 

Data  objects: 

The  following  table  describes  the  generic  formal  objects  required  by  this  part: 


*0 

u  f  U  *f"  M  ^  m  W* 

<  X  "  *  "  X  *  «  «  «  *  » 


** 


— .*r_  -  *V.  w*  •  -  - 


V  *<%  ** 
fy  .* / 

»  W  .  *  * 

•  *  -v 
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ft 

v 


H 


|  Name  |  Type 


Value 


Description 


Rowl 


Row2 


Row3 


Coll 


Col2 


Col3 


Row  Indices 


Row  Indices 


Row  Indices 


Col  Indices 


Col  Indices 


Col  Indices 


Rotation 

Indices 


Rotation 

Indices 


Rotation 

Indices 


’  FIRST 


SUCC 

'FIRST 


'LAST 


'FIRST 


SUCC 

'FIRST 


'LAST 


’  FIRST 


SUCC 

'FIRST 


’LAST 


Object  to  be  used  as  an  index  representing 
the  first  row  of  the  direction  cosine 
matrix.  This  object  indexes  the  direction 
cosines  of  the  angles  between  the  x-axis 
and  the  XYZ  axes. 

Object  to  be  used  as  an  index  representing 
the  second  row  of  the  direction  cosine 
matrix.  This  object  indexes  the  direction 
cosines  of  the  angles  between  the  y-axis 
and  the  XYZ  axes. 

Object  to  be  used  as  an  index  representing 
the  third  row  of  the  direction  cosine 
matrix.  This  object  indexes  the  direction 
cosines  of  the  angles  between  the  z-axis 
and  the  XYZ  axes. 

Object  to  be  used  as  an  index  representing 
the  first  column  of  the  direction  cosine 
matrix.  This  object  indexes  the  direction 
cosines  of  the  angles  between  the  X-axis 
and  the  xyz  axes. 

Object  to  be  used  as  an  index  representing 
the  second  column  of  the  direction  cosine 
matrix.  This  object  indexes  the  direction 
cosines  of  the  angles  between  the  Y-axis 
and  the  xyz  axes. 

Object  to  be  used  as  an  index  representing 
the  third  column  of  the  direction  cosine 
matrix.  This  object  indexes  the  direction 
cosines  of  the  angles  between  the  Z-axis 
and  the  xyz  axes. 

Object  to  be  used  as  an  index  representing 
the  first  element  of  the  rotation  angle 
vector. 

Object  to  be  used  as  an  index  representing 
the  second  element  of  the  rotation  angle 
vector. 

Object  to  be  used  as  an  index  representing 
the  third  element  of  the  rotation  angle 
vector. 


Subprograms : 

The  following  table  describes  the  generic  formal  subroutines  required  by  this 
part: 


W 1 
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|  Name 

1  Type 

Description  | 

|  »*» 

1 

1 

|  function 

1 

1 

This  function  multiplies  an  object  of  type  | 

Sin  Cos_Ratio  to  an  object  of  type  Sin  Cos  Ratio  | 

returning  an  object  of  type  Rotation_Angles.  j 

FORMAL  PARAMETERS: 

The  following  table  describes  this  part's  formal  parameters: 


|  Name  |  Type  |  Mode  |  Description 


DCM  Matrix 


REF_DC 

Matrix 


Direction 

_Cosine 

_Matrix 

Direction 

_Cosine 

Matrix 


In  |  This  parameter  is  the  current  active 
|  direction  cosine  matrix. 


In  j  This  parameter  is  the  reference  direction 
I  cosine  matrix.  The  misalignment  angles 
j  are  computed  by  determining  the 
j  angular  differences  between  this  DCM 
and  the  active  DCM. 


3.3.2.4.9.1.10.4.4  LOCAL  DATA 
Data  objects: 

The  following  table  describes  the  data  objects  maintained  by  this  part: 


|  Name 

|  Type  |Value 

Description  | 

|  Rot_Angle 

1 

|Rotation|  N.A. 
i  Angle  j 
j  _Vec  j 

This  object  is  the  rotation  angle  vector  | 

that  is  returned  by  the  function.  j 

1 

3.3.2.4.9.1.10.4.5  PROCESS  CONTROL 
Not  applicable. 


3.3.2.4.9.1.10.4.6  PROCESSING 

The  following  describes  the  processing  performed  by  this  part: 

separate  (Direction  Cosine_Matrix_Operations.DCM_General_Operations) 
function  Frame_MisaIignment 

(DCM_Matrix  :  Direction_Cosine_Matrix; 
REF_DC_Matrix  :  Direction_Cosine_Matrix) 
return  Rotation_Angle_Vec  is 
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—  — declaration  section 


—  — <the  rotation  angle  vector  that  is  returned> — 
Rot_Angle  :  Rotation_Angle_Vec; 


— begin  function 


begin 

Rot_Angle(X)  :=  DCM_Matrix(Rowl,Col2)  *  REF_DC_Matrix(Rowl,Col3)  + 
DCM_Matrix(Row2,Col2)  *  REF_DC_Matrix(Row2 , Col3)  + 
DCM_Matrix(Row3,Col2)  *  REF_DC_Matrix(Row3,Col3) ; 

Rot_Angle(Y)  :=  DCM_Matrix(Rowl,Col3)  *  REF_DC_Matrix(Rowl,Coll)  + 
DCM_Matrix(Row2,Col3)  *  REFJDC_Matrix(Row2,Coll)  + 
DCM_Matrix(Row3,Col3)  *  REF_DC_Matrix(Row3,Coll) ; 

Rot_Angle(Z)  :=  DCM_Matrix(Rowl,Coll)  *  REF_DC_Matrix(Rowl,Col2)  + 
DCM_Matrix(Row2,Coll)  *  REF_DC_Matrix(Row2,Col2)  + 
DCM_Matrix(Rov3,Coll)  *  REF_DC_Matrix(Row3,Col2) 5 

return  Rot_Angle; 
end  Frame_Misalignment ; 


3.3.2.4.9.1.10.4.7  UTILIZATION  OF  OTHER  ELEMENTS 
None. 


3.3.2.4.9.1.10.4.8  LIMITATIONS 


None. 


3.3.2.4.9.1.10.5  ALIGNED_DCM_MATRIX  UNIT  DESIGN  (CATALOG  #P293-0) 

This  part  modifies  the  bottom  two  rows  (y  and  z)  of  the  direction  cosine  matrix 
so  that  the  current  coordinate  frame  is  aligned  to  a  true  coordinate  frame. 

This  part  assumes  a  coordinate  frame  representation  of  xyz  to  XYZ,  where  xyz  is 
the  coordinate  frame  noted  as  "true".  NOTE:  Only  rows  2  and  3  are  aligned. 

Row  1  must  be  computed  separately. 


3.3.2.4.9.1.10.5.1  REQUIREMENTS  ALLOCATION 
N/A 


“  V 
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3.3.2.4.9.1.10.5.2  LOCAL  ENTITIES  DESIGN 
None. 


3.3.2.4.9.1.10.5.3  INPUT/OUTPUT 
GENERIC  PARAMETERS: 

Data  types: 

The  following  table  describes  the  generic  formal  types  required  by  this  part: 


|  Name  j 

Type 

Description  | 

|  Row  Indices  | 

l 

discrete 

type 

Data  type  which  translates  into  the  j 

row  indexing  of  the  direction  cosine  matrix,  j 

1  1 

j  Col  Indices  | 

i  "  1 

1  1 

1  1 

discrete 

type 

Data  type  which  translates  into  the  j 

column  indexing  of  the  direction  cosine  j 

matrix.  j 

|  Sin_Cos  Ratio | 

floating 

point 

Type  which  translates  into  the  type  of  the  ] 

elements  of  the  direction  cosine  matrix.  | 

1  i 
j  Direction  j 
j  Cosine  j 
!  _Matrix  | 

3X3 

matrix 

Type  representing  a  direction  cosine  matrix  j 

indexed  by  the  column  and  row  types  described] 
above  with  elements  of  type  Sin_Cos_Ratio.  j 

1  1 
j  Rotation  | 

|  _Indices  j 

discrete 

type 

Data  type  which  translates  into  the  ] 

indexing'  elements  of  a  rotation  angle  vector.  | 

1  1 

|  Rotation  j 

j  _Angles  j 

floating 

point 

Type  which  translates  into  the  type  of  the  j 

elements  of  a  rotation  angle  vector.  j 

1  i 

|  Rotation  | 

j  _Angle  ! 

j  _Vec  | 

1X3  matrix 
(vector) 

Type  representing  a  rotation  angle  vector.  j 

Data  objects: 

The  following  table  describes  the  generic  formal  objects  required  by  this  part: 
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|  Name 
Rowl 


I  Type 


Value  |  Description 


I 


Coll 


Row  Indices 


r FIRST 


i 

Row2  |  Row  Indices 

I 


Rqv3  I  Pnu  Tnrliroc: 

i  "  . 


succ 

' FIRST 


r  LAST 


Col  Indices 


'  FIRST 


Col2  |  Col  Indices 

! 

I 


SUCC 

'FIRST 


Col3  |  Col_Indices  |  'LAST 

I 


Rotation 

Indices 


Rotation 

Indices 


I 


|  Rotation 
i  _Indices 

I 


'  FIRST 


SUCC 
'FIRSTI 


'LAST 


Object  to  be  used  as  an  index  representing 
the  first  row  of  the  direction  cosine 
matrix.  This  object  indexes  the  direction 
cosines  of  the  angles  between  the  x-axis 
and  the  XYZ  axes. 

Object  to  be  used  as  an  index  representing 
the  second  row  of  the  direction  cosine 
matrix.  This  object  indexes  the  direction 
cosines  of  the  angles  between  the  y-axis 
and  the  XYZ  axes. 

Object  to  be  used  as  an  index  representing 
the  third  row  of  the  direction  cosine 
matrix.  This  object  indexes  the  direction 
cosines  of  the  angles  between  the  z-axis 
and  the  XYZ  axes. 

Object  to  be  used  as  an  index  representing 
the  first  column  of  the  direction  cosine 
matrix.  This  object  indexes  the  direction 
cosines  of  the  angles  between  the  X-axis 
and  the  xyz  axes. 

Object  to  be  used  as  an  index  representing 
the  second  column  of  the  direction  cosine 
matrix.  This  object  indexes  the  direction 
cosines  of  the  angles  between  the  Y-axis 
and  the  xyz  axes. 

Object  to  be  used  as  an  index  representing 
the  third  column  of  the  direction  cosine 
matrix.  This  object  indexes  the  direction 
cosines  of  the  angles  between  the  Z-axis 
and  the  xyz  axes. 

Object  to  be  used  as  an  index  representing 
the  first  element  of  the  rotation  angle 
vector. 

Object  to  be  used  as  an  index  representing 
the  second  element  of  the  rotation  angle 
vector. 

Object  to  be  used  as  an  index  representing 
the  third  element  of  the  rotation  angle 
vector. 


Subprograms: 

The  following  table  describes  the  generic  formal  subroutines  required  by  this 
part: 
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|  Name 

1  Type 

Description  | 

|  »*» 

1 

1 

|  function 

1 

This  function  multiplies  an  object  of  type  1 

Sin_Cos_Ratio  to  an  object  of  type  Rotation  Angles  j 
returning  an  object  of  type  Sin  Cos  Ratio.  j 

FORMAL  PARAMETERS: 

The  following  table  describes  this  part's  formal  parameters: 


|  Name  |  Type  |  Mode  |  Description 


|  In  |  This  parameter  is  the  current  active 
j  j  direction  cosine  matrix, 

l  l 

I  I 

I  I 

|  In  j  This  parameter  represents  the  rotation 
j  j  angles  used  to  align  the  DCM  matrix. 


3.3.2.4.9.1.10.5.4  LOCAL  DATA 
Data  objects: 

The  following  table  describes  the  data  objects  maintained  by  this  part: 


|  Name  |  Type  | Value |  Description 


|  DCM_New  | Direction | N. A.  |  This  object  is  the  direction  cosine  matrix  | 
!  j  _Cosine  j  j  that  is  computed  and  returned.  j 
I  I  .Matrix  |  |  | 


3.3.2,4,9,1.10,5,5  PROCESS  CONTROL 
Not  applicable. 


3.3.2.4.9.1.10.5.6  PROCESSING 

The  following  describes  the  processing  performed  by  this  part: 

separate  (Direction_Cosine_Matrix_Operations.DCM_General_Operations) 
function  Aligned_DCM_Matrix 

(DCM_Matrix  :  Direction_Cosine_Matrix; 

Rotation_Angle  :  Rotation_Angle_Vec) 
return  Direction  Cosine  Matrix  is 


|  DCM_Matrix  |  Direction 

j  j  _Cosine 

!  I  Mafriy 

I  I 

I  I 

|  Rotation  |  Rotation 

j  _Angle  j  _Angle 

j  j  Vec 


-  — declaration  section 
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- <computed  DCM  to  be  returned> - 

DCM  New  :  Direction  Cosine  Matrix; 


— begin  function 


begin 

DCM_New(Row2,Coll)  :=  DCM_Matrix(Row2,Coll)  +  DCM_Matrix(Row2,Col3)  * 

Rotation_Angle(Y) ; 

DCM_New(Row2 , Col2 )  :=  DCM_Matrix(Row2,Col2)  -  DCM_Matrix(Row2,Col3)  * 

Rotation_Angle(X); 

DCM_New(Row2,Col3)  :=  DCM_Matrix(Row2,Col3)  -  DCM_Matrix(Row2,Coll)  * 

Rotation_Angle(Y)  + 

DCM_Matrix(Row2,Col2)  *  Rotation_Angle(X) ; 

DCM_New(Row3,Coll)  :=  DCM_Matrix(Row3,Coll)  +  DCM_Matrix(Row3,Col3)  * 

Rotation_Angle(Y) ; 

DCM_New(Rov3 , Col2)  ;=  DCM_Matrix(Row3,Col2)  -  DCM_Matrix(Row3,Col3)  * 

Rotation_Angle(X) ; 

DCM_Nev(Rov3,Col3)  :  =  DCM_Matrix(Row3,Col3)  -  DCM_Matrix(Row3,Coll)  * 

Rotation  Angle(Y)  + 

DCM_MatrIx(Row3,Col2)  *  RotationAngle(X) ; 

- <set  first  row  equal  to  original  matrix> — 

- <N0TE:  The  first  row  is  now  INVALID  data> — 

DCM_New(Rowl,Coll)  :=  DCM_Matrix(Rowl,Coll) ; 

DCM_New(Rowl , Col2 )  :=  DCM  Matrix(Rowl,Col2) ; 

DCM_New(Rowl,Col3)  :=  DCM_Matrix(Rowl,Col3); 

return  DCM_New; 

end  Aligned_DCM_Matrix; 


3.3.2.4.9.1.10.5.7  UTILIZATION  OF  OTHER  ELEMENTS 
None.. 


3.3.2.4.9.1.10.5.8  LIMITATIONS 
None. 
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3.3.2.4.9.1.10.6  C0MPUTE_FIRST_R0W_FR0M_0RTH0N0RMAL  UNIT  DESIGN  (CATALOG  #P294-0) 

This  part  computes  the  first  row  of  the  direction  cosine  matrix  from  the  second 
and  third  rows  of  the  same  matrix  using  the  orthonormal  property. 

In  many  applications,  the  second  and  third  rows  are  updated  at  a  higher  rate 
than  the  first  row.  Because  of  this,  this  function  has  been  designed  as  a 
separate  part. 


3.3.2.4.9.1.10.6.1  REQUIREMENTS  ALLOCATION 
N/A 


3.3.2.4.9.1.10.6.2  LOCAL  ENTITIES  DESIGN 


None. 


3.3.2.4.9.1.10.6.3  INPUT/OUTPUT 
GENERIC  PARAMETERS: 

Data  types: 

The  following  table  describes  the  generic  formal  types  required  by  this  part: 


Name  j 

Type 

Description 

Row  Indices  | 

1 

1 

discrete 

type 

Data  type  which  translates  into  the 

row  indexing  of  the  direction  cosine  matrix. 

1 

Col  Indices 

~  i 

| 

discrete 

type 

Data  type  which  translates  into  the 

column  indexing  of  the  direction  cosine 
matrix. 

Sin  Cos  Ratio] 

1 

1 

floating 

point 

Type  which  translates  into  the  type  of  the 
elements  of  the  direction  cosine  matrix. 

Direction  j 

_Cosine  j 

_Matrix  j 

3X3 

matrix 

Type  representing  a  direction  cosine  matrix 
indexed  by  the  column  and  row  types  described 
above  with  elements  of  type  Sin_Cos_Ratio. 

Data  objects: 

The  following  table  describes  the  generic  formal  objects  required  by  this  part: 


LWkiTku  hm. Jy  \ 
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Name 


Type 


Value  |  Description 


Rowl  | 

I 

I 


Row  Indices 


' FIRST 


Object  to  be  used  as  an  index  representing 
the  first  row  of  the  direction  cosine 
matrix.  This  object  indexes  the  direction 
cosines  of  the  angles  between  the  x-axis 
and  the  XYZ  axes. 


Row2 


Row  Indices 


SUCC  |  Object  to  be  used  as  an  index  representing 
'FIRST  j  the  second  row  of  the  direction  cosine 

!  matrix.  This  object  indexes  the  direction 
|  cosines  of  the  angles  between  the  y-axis 
!  and  the  XYZ  axes. 


Row3 


Row  Indices 


'LAST 


Object  to  be  used  as  an  index  representing 
the  third  row  of  the  direction  cosine 
matrix.  This  object  indexes  the  direction 
cosines  of  the  angles  between  the  z-axis 
and  the  XYZ  axes. 


Coll 


Col  Indices 


' FIRST 


Object  to  be  used  as  an  index  representing 
the  first  column  of  the  direction  cosine 
matrix.  This  object  indexes  the  direction 
cosines  of  the  angles  between  the  X-axis 
and  the  xyz  axes. 


Col2 


Col  Indices 


SUCC  j  Object  to  be  used  as  an  index  representing 
'FIRST  j  the  second  column  of  the  direction  cosine 
|  matrix.  This  object  indexes  the  direction 
j  cosines  of  the  angles  between  the  Y-axis 
j  and  the  xyz  axes. 


Col3 


Col  Indices 


'LAST 


Object  to  be  used  as  an  index  representing 
the  third  column  of  the  direction  cosine 
matrix.  This  object  indexes  the  direction 
cosines  of  the  angles  between  the  Z-axis 
and  the  xyz  axes. 


FORMAL  PARAMETERS: 

The  following  table  describes  this  part's  formal  parameters: 


j  Name 

Type 

|  Mode 

Description 

1 

|  DCM  Matrix 

Direction 

1  m 

This  parameter  is  the  current  active 

1 

i 

Cosine 

!  Out 

direction  cosine  matrix. 

1 

Matrix 

1 
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3.3.2.4.9.1.10.6.4  LOCAL  DATA 
None. 

3.3.2.4.9.1.10.6.5  PROCESS  CONTROL 
Not  applicable. 

3.3.2.4.9.1.10.6.6  PROCESSING 

The  following  describes  the  processing  performed  by  this  part: 

separate  (Direction_Cosine_Matrix_Operations.DCM_General_Operations) 
procedure  Compute_First_Row_From_Orthonormal 

(DCM_Matrix  :  in  out  Direction_Cosine_Matrix)  is 

begin 

DCM_Matrix(Rowl,Coll)  :=  DCM_Matrix(Rov2,Col2)  *  DCM_Matrix(Row3,Col3) 

-  DCM_Matrix(Row2 , Col3)  *  DCM_Hatrix(Row3,Col2) ; 

DCM_Matrix(Rowl,Col2)  :=  DCM_Matrix(Row2,Col3)  *  DCM_Matrix(Row3,Coll) 

-  DCM_Matrix(Rov2 , Coll )  *  DCM_Matrix(Rov3,Col3) ; 

DCM_Matrix(Rowl , Col3)  :=  DCM_Matrix(Rov2,Coll)  *  DCM_Matrix(Row3,Col2) 

-  DCM_Ma t rix ( Row2 , Col2 )  *  DCM_Matrix(Rov3,Coll) ; 

end  Compu te_Fi rs t_Row_From_0r thonormal $ 

3.3.2.4.9.1.10.6.7  UTILIZATION  OF  OTHER  ELEMENTS 
None. 

3.3.2.4.9.1.10.6.8  LIMITATIONS 
None. 

3.3.2.4.9.1.10.7  DCM_FROM_QUATERNION  UNIT  DESIGN  (CATALOG  #P295-0) 

This  part  computes  the  direction  cosine  matrix  that  transforms  data  from 
coordinate  frame  xyz  to  XYZ  given  the  unit  quaternion  that  rotates  the  XYZ 
frame  into  the  xyz  frame. 

3.3.2.4.9.1.10.7.1  REQUIREMENTS  ALLOCATION 
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3.3.2.4.9.1.10.7.2  LOCAL  ENTITIES  DESIGN 
None. 

3.3.2.4.9.1.10.7.3  INPUT/OUTPUT 
GENERIC  PARAMETERS: 

Data  types: 

The  following  table  describes  the  generic  formal  types  required  by  this  part: 


|  Name 


I  Type 


|  Description 


I 


j  Row  Indices  j 

discrete 

k 

type 

w 

w 

!  1 
|  Col  Indices  | 

discrete 

5 A1 

s 

i  i 

1  i 

type 

1  1 

V 

1  ! 

£ 

JS; 

,hV 

j  Sin  Cos  Ratio j 

\  1 

floating 

point 

£ 

I  I 

|  Direction  [ 

3X3 

>mm 

|  Cosine  j 

matrix 

j  _Matrix  | 

$ 

■\j 

j  Quaternion  j 

discrete 

$ 

58 

j  Indices  j 

type 

I 

|  Quaternion  | 

1X4 

|  Vector  i 

matrix 

Data  type  which  translates  into  the 

row  indexing  of  the  direction  cosine  matrix. 

Data  type  which  translates  into  the 

column  indexing  of  the  direction  cosine 
matrix. 

Type  which  translates  into  the  type  of  the 
elements  of  the  direction  cosine  matrix. 

Type  representing  a  direction  cosine  matrix 
indexed  by  the  column  and  row  types  described | 
above  with  elements  of  type  Sin_Cos_Ratio. 

Data  type  which  translates  into  the 
indexing  elements  which  index  the 
quaternion. 

Type  representing  the  unit  quaternion  which 
rotates  the  XYZ  frame  into  the  xyz  frame. 


Data  objects; 

The  following  table  describes  the  generic  formal  objects  required  by  this  part: 


*ia  *  |1 


p 


V 

s 
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'O 


|  Name  |  Type 


Value  |  Description 


I 


Rowl 


Row2 


Row3 


Coll 


Col2 


Col3 


QO 

Q1 


Q2 


Q3 


Row  Indices 


Row  Indices 


Row  Indices 


Col  Indices 


Col  Indices 


Col  Indices 


Quaternion 

Indices 


Quaternion 

Indices 


Quaternion 

Indices 


Quaternion 

Indices 


'  FIRST 


SUCC 

'FIRST 


'LAST 


'FIRST 


SUCC 

'FIRST 


'LAST 


'FIRST 


SUCC 

'FIRST 


PRED 

'LAST 


'LAST 


Object  to  be  used  as  an  index  representing 
the  first  row  of  the  direction  cosine 
matrix.  This  object  indexes  the  direction 
cosines  of  the  angles  between  the  x-axis 
and  the  XYZ  axes. 


Object  to  be  used  as  an  index  representing 
the  second  row  of  the  direction  cosine 
matrix.  This  object  indexes  the  direction 
cosines  of  the  angles  between  the  y-axis 
and  the  XYZ  axes. 


Object  to  be  used  as  an  index  representing 
the  third  row  of  the  direction  cosine 
matrix.  This  object  indexes  the  direction 
cosines  of  the  angles  between  the  z-axis 
and  the  XYZ  axes. 


Object  to  be  used  as  an  index  representing 
the  first  column  of  the  direction  cosine 
matrix.  This  object  indexes  the  direction 
cosines  of  the  angles  between  the  X-axis 
and  the  xyz  axes. 


Object  to  be  used  as  an  index  representing 
the  second  column  of  the  direction  cosine 
matrix.  This  object  indexes  the  direction 
cosines  of  the  angles  between  the  Y-axis 
and  the  xyz  axes. 


Object  to  be  used  as  an  index  representing 
the  third  column  of  the  direction  cosine 
matrix.  This  object  indexes  the  direction 
cosines  of  the  angles  between  the  Z-axis 
and  the  xyz  axes. 


Object  to  be  used  as  an  index  representing 
the  first  element  of  the  quaternion. 


Object  to  be  used  as  an  index  representing 
the  second  element  of  the  quaternion. 


Object  to  be  used  as  an  index  representing 
the  third  element  of  the  quaternion. 


Object  to  be  used  as  an  index  representing 
the  fourth  element  of  the  quaternion. 


FORMAL  PARAMETERS: 


The  following  table  describes  this  part's  formal  parameters: 


Y, 

\ A 


V** 


CAMP  Software  Detailed  Design  Document 


Page  258 


|  Name 

1  Type 

|  Mode  |  Description  | 

|  Quaternion 

1 

|  Quaternion 

5  _Vectors  j 

In  |  This  object  represents  the  unit  j 

i  quaternion  which  rotates  XYZ  into  xyz.j 

3.3.2.4.9.1.10.7.4  LOCAL  DATA 
Data  objects: 


a 


The  following  table  describes  the  data  objects  maintained  by  this  part: 


|  Name 

|  Type  | Value | 

Description 

1 

I  Q0  Sqr 

|Sin  Cos  | 

N.A.  | 

This  object  temporarily  hold 

the 

square  of 

1 

|  Ratio  j 

1 

quaternion  element  qO. 

1 

|  Ql  Sqr 

!  1 
| Sin  Cos  | 

N.A.  | 

This  object  temporarily  hold 

the 

square  of 

1 

1 

1  Ratio  | 

1 

quaternion  element  ql. 

1 

1 

|  Q2  Sqr 

1  1 
| Sin  Cos  | 

N.A.  | 

This  object  temporarily  hold 

the 

square  of 

1 

1 

j  _Ratio  j 

quaternion  element  q2. 

1 

1 

|  Q3  Sqr 

1  1 
|Sin  Cos  | 

N.A.  | 

This  object  temporarily  hold 

the 

square  of 

1 

j  _Ratio  j 

quaternion  element  q3. 

1 

|  DCM 

1  ! 

| Direction! 

N.A.  | 

This  object  is  the  direction 

cosine  matrix 

1 

t  Cosine  j 

that  is  computed  and  returned. 

1 

j  _Matrix  j 

1 

1 

3.3.2.4.9.1.10.7.5  PROCESS  CONTROL 
Not  applicable. 


3.3.2.4.9.1.10.7.6  PROCESSING 

The  following  describes  the  processing  performed  by  this  part: 

separate  (Direction_Cosine_Matrix_Operations . DCM_General_Operations) 
function  DCM  From_Quaternion 

^Quaternion  :  Quaternion_Vectors) 
return  Direction  Cosine  Matrix  is 


—  — declaration  section 


—  — <objects  to  hold  the  squares  of  quaternions> — 

Q0_Sqr  :  Sin_Cos_Ratio; 

Ql_Sqr  :  Sin_Cos  Ratio; 


%  v.'.'  s’iv.  v.  . . 
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Q2_Sqr  :  Sin_Cos_Ratio; 

Q3_Sqr  :  Sin_Cos_Ratio; 

—  — <the  DCM  to  be  returned> — 


DCM  :  Direction  Cosine  Matrix; 


— begin  function 


begin 

-  <compute  squares  of  quaternions>  - 

Q0_Sqr  :=  Quaternion(qO)  *  Quaternion(qO); 
Ql_Sqr  :=  Quaternion(ql)  *  Quaternion(ql) ; 
Q2_Sqr  :=  Quaternion(q2)  *  Quaternion(q2) ; 
Q3_Sqr  :=  Quaternion(q3)  *  Quaternion(q3) ; 


—  -  <compute  direction  cosines>  - 

DCM(Rowl,Coll)  :=  Q0_Sqr  +  Ql_Sqr  -  Q2_Sqr  -  Q3_Sqr; 

DCM(Row2,Col2)  :=.Q0_Sqr  -  Q1  Sqr  +  Q2_Sqr  -  Q3_Sqr; 

DCM(Row3,Col3)  :=  Q0_Sqr  -  Ql~Sqr  -  Q2_Sqr  +  Q3_Sqr; 

DCM(Rov2,Coll)  :=  (  (Quaternion(qO)  *  Quaternion(q3)) 

+(Quaternion(ql)  *  Quaternion(q2))  ) 

*  2.0; 

DCM(Row3,Coll)  :=  (  (Quaternion(ql)  *  Quaternion(q3)) 

-(Quaternion(qO)  *  Quaternion(q2))  ) 

*  2.0; 

DCM(Rowl,Col2)  :=  (  (Quaternion(ql)  *  Quaternion(q2)) 

-(Quaternion(qO)  *  Quaternion(q3))  ) 

*  2.0; 

DCM(Rov3,Col2)  :=  (  (Quaternion(qO)  *  Quaternion(ql)) 

+(Quaternion(q2)  *  Quaternion(q3))  ) 

*  2.0; 

DCM(Rowl,Col3)  :=  (  (Quaternion(qO)  *  Quaternion(q2)) 

+(Quaternion(ql)  *  Quaternion(q3))  ) 

*  2.0; 

DCM(Row2,Col3)  :=  (  (Quaternion(q2)  *  Quaternion(q3)) 

-(Quaternion(qO)  *  Quaternion(ql))  ) 

*  2.0; 

return  DCM; 

end  DCM_From_Quaternion; 
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3.3.2.4.9.1.10.7.7  UTILIZATION  OF  OTHER  ELEMENTS 
None. 

3.3.2.4.9.1.10.7.8  LIMITATIONS 
None. 
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3. 3. 2. 4. 9. 2  CNE_0PERATI0NS  PACKAGE  DESIGN  (CATALOG  #P296-0) 

This  package  contains  all  functions  and  procedures  that  specifically  apply  to  a 
CNE  direction  cosine  matrix.  A  CNE  DCM  is  a  3X3  matrix  that  is  used  to 
transform  data  between  the  Navigation  coordinate  frame  and  the  Earth  coordinate 
frame.  The  Navigation  frame  assumes  the  convention  "East,  North,  Up",  and  the 
Earth  frame  assumes  the  convention  "Greenwich,  Right,  Polar".  The  elements  of 
this  matrix  are  direction  cosines  of  angles  between  specific  axes  of  the  two 
coordinate  frames. 

The  decomposition  for  this  part  is  the  same  as  that  shown  in  the  Top-Level 
Design  Document. 


3. 3. 2. 4. 9. 2.1  REQUIREMENTS  ALLOCATION 

The  following  table  summarizes  the  allocation  of  CAMP  requirements  to  this 
part: 


|  Name 


Requirements  Allocation 


CNE_Initialized_From_Reference  |  N/A 
Reorthonormalize_CNE  j  N/A 
Compute_First_Row_of_CNE_From_Orthonormal j  N/A 
CNE  Initialized_From_Earth_Position  j  R32 
Per?orm_TrapezoIdal_Integration_of_CNE  j  N/A 
Perform_Rectangular_Integration_of_CNE  j  N/A 
Frame_Misalignment_of_CNE  j  N/A 
Aligned _CNE_Matrix~  ~  |  N/A 
CNE  From  Quaternion  j  N/A 


3. 3. 2. 4. 9. 2. 2  LOCAL  ENTITIES  DESIGN 


Subprograms : 

The  following  table  describes  the  subprograms  in  this  part.  The  first  two 
subprograms  are  instantiations  of  parts  from  DCM_General_Operations. 
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|  Name 


CNE_Initialized 

_From 

Reference 


Type  | 
function! 


Description 


This  function  initializes  a  CNE  matrix  to  a 
reference  CNE  matrix. 


Reorthonormalize I 
CNE 


Compute_First 

_Row_of_CNE 

_From 

Orthonormal 


procedure]  This  procedure  reorthonormalizes  a  CNE  matrix. 

I 

procedure | 


This  procedure  computes  the  first  row  of  a  CNE 
matix  using  the  orthonormal  property. 


CNE_Initialized 

_From_Earth 

Position 


generic 

function! 


This  function  initializes  the  CNE  matrix  from 
the  latitude,  longitude,  and  wander  angle  of 
the  missile  location. 


3. 3. 2. 4. 9. 2. 3  INPUT/OUTPUT 
GENERIC  PARAMETERS: 

Data  types: 

The  following  table  describes  the  generic  formal  types  required  by  this  part: 


|  Name 


|  Type  |  Description 


Earth  Axes 


discrete 

type 


Data  type  which  translates  into  the 
row  indexing  of  the  direction  cosine 
matrix.  This  type  should  be  equivalent 
(conceptually)  to  "Greenw,  Right,  Polar". 


Navigation 

Axes 


discrete 

type 


Data  type  which  translates  into  the  column 
indexing  of  the  direction  cosine  matrix. 

This  type  should  be  equivalent  (conceptually) 
to  "East,  North,  Up". 


Sin  Cos  Ratio 


floating 

point 


Type  which  translates  into  the  type  of  the 
elements  of  the  direction  cosine  matrix. 


CNE  Matrix 


3X3 

matrix 


Type  representing  a  direction  cosine  matrix 
indexed  by  the  column  and  row  types  described 
above  with  elements  of  type  Sin  Cos  Ratio. 


Data  objects: 

The  following  table  describes  the  generic  formal  objects  required  by  this  part: 


•fs! 
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% 


<* 


Name 

Greenw 


Right 


Polar 


East 


North 


Up 


Type 

Earth  Axes 


|  Value 
'FIRST 


Description 


Earth  Axes 


Earth  Axes 


Navigation 

Axes 


Navigation 

Axes 


Navigation 

Axes 


SUCC 

'FIRST 


'LAST 


'FIRST 


SUCC 

'FIRST 


'LAST 


Object  to  be  used  as  an  index  representing 
the  first  row  of  the  direction  cosine 
matrix.  This  object  indexes  the  direction 
cosines  of  the  angles  between  the  x-axis 
and  the  XYZ  axes. 


Object  to  be  used  as  an  index  representing 
the  second  row  of  the  direction  cosine 
matrix.  This  object  indexes  the  direction 
cosines  of  the  angles  between  the  y-axis 
and  the  XYZ  axes. 


Object  to  be  used  as  an  index  representing 
the  third  row  of  the  direction  cosine 
matrix.  This  object  indexes  the  direction 
cosines  of  the  angles  between  the  z-axis 
and  the  XYZ  axes. 


Object  to  be  used  as  an  index  representing 
the  first  column  of  the  direction  cosine 
matrix.  This  object  indexes  the  direction 
cosines  of  the  angles  between  the  X-axis 
and  the  xyz  axes. 


Object  to  be  used  as  an  index  representing 
the  second  column  of  the  direction  cosine 
matrix.  This  object  indexes  the  direction 
cosines  of  the  angles  between  the  Y-axis 
and  the  xyz  axes. 


Object  to  be  used  as  an  index  representing 
the  third  column  of  the  direction  cosine 
matrix.  This  object  indexes  the  direction 
cosines  of  the  angles  between  the  Z-axis 
and  the  xyz  axes. 


FORMAL  PARAMETERS: 


The  following  table  describes  subprogram  CNE_Initialized_From_Reference's 
formal  parameters: 


jj  # 


M 

-'.S  •>  .'"V  V*  A  , 

«  "  « /V  <r .  "  -  .  ^  «%  *%  >c„  »  .  »  H  ' 


'  "  -AX ■  "  -  *■ 


*.*Vi 


■‘V  A'*'- 
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|  Name 


Type  |  Mode  |  Description 


REF  CNE  2  1 


Sin  Cos  Ratio 


In 


This  value  is  the  reference  direction 
cosine  element  located  at  RIGHT, 
EAST.  It  represents  the 
direction  cosine  of  the  angle 
between  the  y-axis  and  the  X-axis. 


REF_CNE_2_2 


Sin  Cos  Ratio 


In 


This  value  is  the  reference  direction 
cosine  element  located  at  RIGHT, 
NORTH.  It  represents  the 
direction  cosine  of  the  angle 
between  the  y-axis  and  the  Y-axis. 


REF  CNE  3  1 


Sin  Cos  Ratio 


In 


This  value  is  the  reference  direction 
cosine  element  located  at  POLAR, 
EAST.  It  represents  the 
direction  cosine  of  the  angle 
between  the  z-axis  and  the  X-axis. 


REF  CNE  3  2 


Sin  Cos  Ratio 


In 


This  value  is  the  reference  direction 
cosine  element  located  at  POLAR, 
NORTH.  It  represents  the 
direction  cosine  of  the  angle 
between  the  z-axis  and  the  Y-axis. 


Sign_0f_2_3  j  INTEGER 


Sign_Of_3_3  |  INTEGER 


i 

i 


In  j  This  value  represents  the  sign  of  the 
j  reference  direction  cosine  element 
|  located  at  RIGHT,  column  3.  This 
j  value  is  the  sign  of  the  direction 
j  cosine  of  the  angle  between  the 
j  y-axis  and  the  Z-axis.  The  value 
j  must  equal  either  -1  or  1,  which 
j  represents  minus  or  plus, 
j  respectively. 

i 

1 

In  |  This  value  represents  the  sign  of  the 
j  reference  direction  cosine  element 
[  located  at  POLAR,  column  3.  This 
j  value  is  the  sign  of  the  direction 
j  cosine  of  the  angle  between  the 
j  z-axis  and  the  Z-axis.  The  value 
j  must  equal  either  -1  or  1,  which 
|  represents  minus  or  plus, 

[  respectively. 


The  following  table  describes  subprogram  Reorthonormalize_CNE's  formal 
parameters: 


,-A\ 

v\w“> 


|  Name 

1  Type  | 

Mode 

|  Description 

1 

|  CNE 

1 

|  CNE  Matrices | 

1  "  1 

in 

|  This  parameter 
|  matrix  to  be 

is  the  direction  cosine  | 
reorthonormalized.  j 
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3. 3. 2. 4. 9. 2. 4  LOCAL  DATA 


Data  objects: 

The  following  table  describes  the  data  objects  maintained  by  this  part: 


|  Name  |  Type  |  Value  |  Description 


CNE_Ma trices 


3X3 


N.A. 


This  type  is  a  type  representing  a  DCM 
derived  from  the  coordinate  frames  of 
the  Earth  and  Navigation  frames.  The 
earth  frame  axes  are  assumed  to  be 
"Greenwich,  Right,  Polar",  and  the 
navigation  frame  axes  are  assumed  to  be 
"North,  East,  Up". 


3. 3. 2. 4. 9. 2. 5  PROCESS  CONTROL 
Not  applicable. 


3. 3. 2. 4. 9. 2. 6  PROCESSING 

The  following  describes  the  processing  performed  by  this  part: 

separate  (Direction_Cosine_Matri:;_Operations) 
package  body  CNE_Operations  is 

package  DCM_GEN_PKG  renames 

Direction_Cosine_Matrix_Operations.DCM_General_Operations; 


- instantiation  of  a  units  from  "DCM_General_Operations"  with  CNE 

—  —  specific  parameters  applied  to  the  generics> 


function  Initialize_From_Ref  is  new 

DCM  GEN_PKG. DCM_Ini tiali?edJ?rom_Reference 
^Rov_Indices  "*=>  Earth_Axes, 

Col_Indices  =>  Navigation_Axes, 

Sin_Cos_Ratio  =>  Sin_Cos_Ratio, 

Direction_Cosine_Matrix  =>  CNE_Matrices) ; 

procedure  Reorthonormalize  is  new 

DCK_GEN_PKG .Reorthonormaliz  e_DCM 

(Row_Indices  .  =>  Earth_Axes, 

Col_Indices  =>  Navigation_Axes, 

Sin_Cos_Ratio  =>  Sin_Cos_Ratio, 

Direction_Cosine_Matrix  =>  CNE  Matrices, 

Real  =>  Real) ; 

procedure  Compute_First_Row  is  new 

DCM_GEN_PKG.Compute_First_Row_From_Orthonormal 
(Row_Indices  =>  Earth_Axes, 
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Col_Indices  =>  Navigation_Axes, 

Sin_Cos_Ratio  =>  Sin_Cos_Ratio, 

Direction  Cosine  Matrix  =>  CNE  Matrices); 


—  — separate  parts 


function  CNE_Initialized_From_Earth_Position 

(Wander_Angle  :  Angles; 

Latitude  :  Earth_Position; 
Longitude  :  Earth  Position) 
return  CNE_Matrices  Is  separate; 

package  body  CNE_Integration  is  separate; 

package  body  Alignment_Parts  is  separate; 

package  body  CNE_From_Quaternion  is  separate; 


- routines  acting  as  interfaces  to  instantiated  routines 


function  CNE  Initialized_From_Reference 

^Ref_CNE_2_l  :  SinCosJRatio; 

Ref_CNE_2_2  2  Sin~Cos_Ratio; 

Ref_CNE_3~l  j  Sin_Cos_Ratio; 

Ref_CNE_3~2  ;  Sin_CosJRatio; 

Sign_0f_2_3  :  INTEGER; 

Sign_Of_3_3  :  INTEGER) 

return  CNE_Matrices  is 

begin 

return  Initialize_From_Ref 

(Ref_DCM_2_l  =>  Ref_CNE_2_l, 

Re£_DCM_2_2  =>  Ref_CNE  2_2, 

Ref_DCM_3_l  =>  Ref_CNE~3_l, 

Ref_DCM_3  2  =>  Ref_CNE  3_2, 

Sign_0f_2~3  =>  Sign_0f_2_3, 

Sign  0f_3_3  =>  Sign_Of_3_3); 
end  CNE_Ini t ialize3_From_Ref erence ; 

procedure  Reorthonormalize_CNE  (CNE  :  in  out  CNE_Matrices)  is 
begin 

Reorthonormalize  (CNE); 
end  Reorthonormalize_CNE; 

procedure  Compute_First_Row_of_CNE_From_Orthonormal 
(CNE  :  in  out  CNE_Matrices)  is 

begin 

Compute_First_Row  (CNE); 
end  Compute_First_Row_of_CNE_From_Orthonormal; 


end  CNE_Operations; 
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3. 3. 2. 4. 9. 2. 7  UTILIZATION  OF  OTHER  ELEMENTS 


None. 


3. 3. 2. 4. 9. 2. 8  LIMITATIONS 
None. 


3. 3. 2. 4. 9. 2. 9  LLCSC  DESIGN 

3. 3. 2. 4. 9. 2. 9.1  CNE_INTEGRATION  PACKAGE  DESIGN  (CATALOG  #P298-0) 

This  generic  package  contains  procedures  that  integrate  a  CNE  direction  cosine 
matrix.  The  first  two  procedures  initialize  and  integrate  a  CNE  using 
trapezoidal  integration.  The  last  procedure  integrates  a  CNE  using  rectangular 
integration.  Note  that  the  first  row  is  not  computed  in  the  integration.  It 
must  be  computed  by  another  function. 

The  decomposition  for  this  part  is  the  same  as  that  shown  in  the  Top-Level 
Design  Document. 


3. 3. 2. 4. 9. 2. 9. 1.1  REQUIREMENTS  ALLOCATION 

The  following  table  summarizes  the  allocation  of  CAMP  requirements  to  this 
part: 


|  Name 


Requirements  Allocation 


|  Perform_Trapezoidal_Integration_of_CNE|  N/A 

j  Perform_Rectangular_Integration_of_CNE|  N/A 


3. 3. 2. 4. 9. 2. 9. 1.2  LOCAL  ENTITIES  DESIGN 


Packages : 

For  "Perform_Trapezoidal_Integration_of_CNEn,  the  package  "Integrate_Trap"  is 
created  and  maintained. 

Subprograms : 

For  "Perform_Rectangular_Integration_of_CNE",  the  procedure  ''Integrate_Rect"  is 
created  and  maintained. 


3. 3. 2. 4. 9. 2. 9. 1.3  INPUT/OUTPUT 


GENERIC  PARAMETERS: 


Data  types: 
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The  following  table  describes  the  generic  formal  types  required  by  this  part: 


|  Name 


|  Description 


Angular  Velocities 


Time  Intervals 


floating 

point 


floating 

point 


Data  type  that  translates  into  the  type  of 
a  component  of  the  angular  velocity  in 
a  given  reference  frame. 

Data  type  that  translates  into  the  type  of 
an  object  that  represents  a  time 
interval  (delta  time). 


Data  objects: 

The  following  table  describes  the  generic  formal  objects  required  by  this  part: 


i  Name 


|  Type  |  Description 


Initial  East  Rho 


Initial  North  Rho 


Angular 

Velocities 


Angular 

Velocities 


This  object  can  be  used  to  initialize 
the  East  component  of  angular  velocity 
(for  trapezoidal  integration). 

This  object  can  be  used  to  initialize 
the  North  component  of  angular  velocity 
(for  trapezoidal  integration). 


Subprograms : 

The  following  table  describes  the  generic  formal  subroutines  required  by  this 
part: 


|  Name  |  Type  |  Description 


This  function  computes  the  product  of  an  object  of  type| 
Angular_Veiocity  and  an  object  of  type  Time_Intervals j 
producing  an  object  of  type  Sin_Cos_Ratio.  j 

This  function  computes  the  product  of  an  object  of  typej 
Angular_Velocity  and  an  object  of  type  Sin_Cos_Ratio  j 
producing  an  object  of  type  Angular_Velocities.  j 


FORMAL  PARAMETERS: 

The  following  table  describes  procedure  i,Perform_Trapezoidal_Integration_of_~ 
CNE"'s  formal  parameters:  ~ 
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Name 

1  Type  | 

Mode 

|  Description 

CNE 

|  CNE  1 

j  _Matricesj 

In 

Out 

|  This  parameter  is  the  direction  cosine 
[  matrix  that  is  integrated. 

l 

East_Rho 

1  1 

| Angular  j 

j_Velocities  j 

In 

1 

|  This  value  is  the  East  component  of 
i  angular  velocity, 
i 

North  Rho 

| Angular  j 

j_Velocities  j 

In 

1 

|  This  value  is  the  North  component  of 
j  angular  velocity. 

i 

Delta  Time 

1  1 

|Time  [ 

j  _Intervals| 

In 

|  This  value  is  the  time  interval  which 

1  equals  the  time  since  the  last 
j  integration. 

The  following  table  describes  procedure  "Reinit_Ang_Vel_For_Trapez_Integ  of_- 
CNE"'s  formal  parameters:  ~  “  ~ 


|  Name 
|  East_Rho 

I  North  Rho 


Type  |  Mode  |  Description 

gular  I  In  I  This  value  i 


Angular 

Velocities 


Angular 

Velocities 


This  value  is  the  East  component  of 
angular  velocity  to  be  used  for 
initialization. 

This  value  is  the  North  component  of 
angular  velocity  to  be  used  for 
initialization. 


The  following  table  describes  procedure  "Perform_Rectangular_Integration_of_- 
CNE"'s  formal  parameters: 


|  Name 


|  Type  |  Mode  |  Description 


CNE 

CNE  j 

_Matricesj 

In 

Out 

|  This  parameter  is  the  direction  cosine 
|  matrix  that  is  integrated. 

1 

East_Rho 

1 

Angular  | 

_Velocities | 

In 

1 

|  This  value  is  the  East  component  of 
i  angular  velocity. 

1 

North_Rho 

I 

Angular  j 

_Velocitiesj 

In 

1 

|  This  value  is  the  North  component  of 
|  angular  velocity. 

1 

Delta_Time 

1 

Time  | 

_Intervals  j 

In 

1 

|  This  value  is  the  time  interval  which 
!  equals  the  time  since  the  last 
i  integration. 

CAMP  Software  Detailed  Design  Document 


Page  270 


3. 3. 2. 4. 9. 2. 9. 1.4  LOCAL  DATA 
None. 


3. 3. 2. 4. 9. 2. 9. 1.5  PROCESS  CONTROL 
Not  applicable. 


3. 3. 2. 4. 9. 2. 9. 1.6  PROCESSING 

The  following  describes  the  processing  performed  by  this  part: 


separate  (Direction_Cosine_Matrix_Operations.CNE_Operations) 
package  body  CNE  Integration  is 


- local  instantiations 


— instantiation  of  a  units  from  ,,DCM_General_Operations"  with  CNE 
—  specific  parameters  applied  to  the  generics> — 


package  Integrate_Trap  is  new. 

DCM_GEN  PKG.DCM_Trapezoidal_Integration 


(AnguIar_Veloci ties 
Time_Intervals 
Row_Indices 
Col_Indices 
Sin_Cos_Ratio 
Direction_Cosine_Matrix 
Initial_X_Rho 
Initial  Y  Rho 


=>  Angular_Velocities, 
=>  Time  Intervals, 

«>  EartH_Axes, 

=>  Navigation_Axes, 

=>  Sin_Cos_Ratio, 

=>  CNE_Mat rices, 

=>  Initial_East  Rho, 

=>  Initial  NortK  Rho); 


procedure  Integrate_Rect  is  new 

DCM_GEN  PKG.Perforra_Rectangular_Integration_of_DCM 
(AnguIar_Velocities  =>  Angular_Velocities, 
Time_Intervals  =>  Time  Intervals, 

Row_Indices  =>  EartE_Axes, 

Col_Indices  =>  Navigation_Axes, 

Sin_Cos_Ratio  =>  Sin_Cos_Ratio, 

Direction  Cosine  Matrix  =>  CNE  Matrices); 


- routines  acting  as  interfaces  to  instantiated  routines 


•[Trapezoidal  Integration]- 


procedure  Perform_Trapezoidal_Integration_of_CNE 

(CNE  :  in  out  CNE  Matrices; 


begin 


East  Rho 

:  in 

Angular_Veloci ties ; 

NortE_Rho 

:  in 

Angular_Velocities; 

Delta_Time 

:  in 

Time_Intervals)  is 

Integrate_Trap.Perform_Trapezoidal_Integration_of_DCM 


V*  *T'  *  *  ’*.*1  m 
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(CNE,  East_Rho,  North_Rho,  Delta_Tirae) ; 
end  Perform  Trapezoidal  Integration  of  CNE; 


procedure  Reini t_Ang_Vel_f or_Trapez_Integ_of_CNE 

(East_Rho  :  in  Angular_Velocities; 

North_Rho  :  in  Angular_Velocities)  is 

begin 

In  tegrate_Trap . Reini tialize_Angular_Veloci ties 

(East_Rho,  North_Rho); 
end  Reini t_Ang_Vel_f or_Trapez_Integ_of_CNE ; 


-[Rectangular  Integration]- 


procedure  Perform_Rectangular_Integration_of_CNE 
(CNE  :  in  out  CNE_Matrices; 

East  Rho  :  in  Angular_Velocities; 

NortE_Rho  :  in  Angular_Velocities; 

Delta_Time  :  in  Time_Intervals)  is 

begin 

Integrate_Rect  (CNE,  East_Rho,  North_Rho,  Delta_Time); 
end  Perform  Rectangular  Integration  of  CNE; 


end  CNE  Integration; 


3. 3. 2. 4. 9. 2. 9. 1.7  UTILIZATION  OF  OTHER  ELEMENTS 

UTILIZATION  OF  OTHER  ELEMENTS  IN  TOP  LEVEL  COMPONENT: 

The  following  tables  describe  the  elements  used  by  this  part  but  defined 
elsewhere  in  the  parent  top  level  component: 


Packages : 


|  Type  |  Source  |  Description 


DCM_General 

Operations 


DCM 

_Trapezoidal 
_Integration 
of  DCM 


package 


package 


Direction 

_Cosine 

_Matrix 

Operations 


DCM_General 

Operations 


This  package  contains  general 
operations  for  direction  cosine 
matrices.  CNE_Integration 
instantiates  functions  and  procedures 
from  this  package. 

This  package  contains  2  procedures;  the 
first  reinitializes  the  angular  vel¬ 
ocities  for  trapezoidal  integration, 
and  the  second  performs  the 
integration. 


Data  types: 
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The  following  table  summarizes  the  types  required  by  this  part  and  defined 
elsewhere  in  the  parent  top  level  component: 


j  Name  | 

Type  | 

Source  j 

Description  | 

|  CNE  | 

j  Matrices! 

1  "  1 

1  i 

3X3  | 

matrix j 

1 

1 

CNE  | 

Operations | 

1 

1 

Type  representing  a  direction  cosine  j 

matrix  indexed  by  the  column  and  row  j 
types  representing  the  navigation  and  j 
earth  frame.  | 

3. 3. 2. 4. 9. 2. 9. 1.8  LIMITATIONS 
None. 


3. 3. 2. 4. 9. 2. 9. 1.9  LLCSC  DESIGN 
None. 


3.3.2.4.9.2.9.1.10  UNIT  DESIGN 
None. 


3. 3. 2. 4. 9. 2. 9. 2  ALIGNMENTJPARTS  PACKAGE  DESIGN  (CATALOG  #P299-0) 

This  package  contains  two  functions.  The  first  function  computes  the  rotation 
angles  between  the  CNE  and  a  reference  direction  cosine  matrix.  The  second 
function  rotates  the  CNE  matrix  into  alignment  by  using  the  rotation  angles. 
The  second  function  only  computes  the  second  and  third  row  of  the  CNE  matrix. 
The  first  row  must  be  updated  separately. 

The  decomposition  for  this  part  is  the  same  as  that  shown  in  the  Top-Level 
Design  Document. 


3. 3. 2. 4. 9. 2. 9. 2.1  REQUIREMENTS  ALLOCATION 

The  following  table  summarizes  the  allocation  of  CAMP  requirements  to  this 
part: 


|  Name 

|  Frame_Misalignment_of_CNE 
|  Aligned_CNE_Matrix 


|  Requirements  Allocation 

|  N/A 

|  N/A 
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3. 3. 2. 4. 9. 2. 9. 2. 2  LOCAL  ENTITIES  DESIGN 


Packages : 

For  "Perform_Trapezoidal_Integration_of_CNE",  the  package  "IntegrateJTrap"  is 
created  and  maintained. 

Subprograms: 

For  "Frame_Misalignment_of_CNE",  the  function  "Computed  _Frame_Misalignment"  is 
created  and  maintained.  For  "Aligned_CNE_Matrix",  the  function  "Align_CNE_- 
Matrix"  is  created  and  maintained. 


3. 3. 2. 4. 9. 2. 9. 2. 3  INPUT/OUTPUT 


GENERIC  PARAMETERS: 

Data  types: 

The  following  table  summarizes  the  generic  formal  types  required  by  this  part: 


|  Name 

1  Type  | 

Description  | 

(t 

|  Rotation 
j  _Indices 

|  discrete  | 

1  type  | 

1  s 

Data  type  which  translates  into  the  | 

indexing  elements  of  a  rotation  angle  vector. j 

• 

1 

|  Rotation 
|  _Angles 

1  1 
|  floating  ! 

|  point  | 

1 

Type  which  translates  into  the  type  of  the  j 

elements  of  a  rotation  angle  vector.  i 

I 

I 

|  Rotation 
|  Angle 
|  _Vec 

1  i 

|  1X3  matrixj 
|  (vector)  | 

1  1 

Type  representing  a  rotation  angle  vector.  j 

! 

1 

Data  objects: 

The  following  table  describes  the  generic  formal  objects  required  by  this  part: 


Name  |  Type 
X 


Value 


Description 


Rotation 

Indices 


Rotation 

Indices 


Rotation 

Indices 


r  FIRST 


SUCC 
' FIRST 


'LAST 


Object  to  be  used  as  an  index  representing 
the  first  element  of  the  rotation  angle 
vector. 

Object  to  bo.  used  as  an  index  representing 
the  second  element  of  the  rotation  angle 
vector. 

Object  to  be  used  as  an  index  representing 
the  third  element  of  the  rotation  angle 
vector. 


VI 

} 


i 

£ 

i 


1 
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The  following  table  describes  the  generic  formal  subroutines  required  by  this 
part: 


|  Name 

M*ll 

«*« 


Type  |  Description 


I 


function  |  This  function  computes  the  product  of  an  object  of  type| 
Sin  Cos_Ratio  and  an  object  of  type  Rotation_Angles 
producing  an  object  of  type  Sin_Cos_Ratio. 

function  i  This  function  computes  the  product  of  an  object  of  typej 
Sin  Cos_Ratio  and  an  object  of  type  Sin_Cos_Ratio 
producing  an  object  of  type  Rotation_Angles. 


FORMAL  PARAMETERS: 

The  following  table  describes  function  "Frame_Misalignment_of_CNE"'s  formal 
parameters: 


|  Name  |  Type  |  Mode  |  Description 


|  CNE  | 

1  1 

CNE  | 

Matrices  j 

~ 

In 

This  parameter  is  the  active  direction  | 
cosine  matrix  (of  the  missile).  I 

| 

! 

|  REF_DC_Matrix| 

CNE  i 

_Matrices  j 

In 

This  parameter  is  the  reference  direction! 
cosine  matrix.  j 

The  following  table  describes  function  "Aligned_CNE_Matrix"'s  formal 
parameters: 


Name 

1  Type 

|  Mode 

Description 

CNE 

|  CNE 

|  In 

This  parameter  is  the  active  direction 

j  _Matrices 

cosine  matrix  (of  the  missile). 

Rotation 

1 

!  Rotation 

1  In 

This  parameter  is  the  rotation  angles  to 

Angle 

1  Angle 

be  used  to  align  the  DCM. 

j  Vec 

3. 3. 2. 4. 9. 2. 9. 2. 4  LOCAL  DATA 
None. 


3. 3. 2. 4. 9. 2. 9. 2. 5  PROCESS  CONTROL 
Not  applicable. 
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3. 3. 2. 4. 9. 2. 9. 2. 6  PROCESSING 

The  following  describes  the  processing  performed  by  this  part: 

separate  (Direction_Cosine_Matrix_Operations.CNE_Operations) 
package  body  Alignment_Parts  is 


—  — local  instantiations 


— instantiation  of  a  unit  from  "DCM_General_Operations"  with  CNE 
—  specific  parameters  applied  to  the  generics> - 

function  Computed_Frame_Misalignment  is  new 
DCM_GEN_PKG . Frame_Misalignment 

(Row_Indices  =>  Earth_Axes, 

Col_Indices  =>  Navigation_Axes, 

Sin_Cos_Ratio  =>  Sin_Cos_Ratio, 

Direction_Cosine_Matrix  =>  CNE_Matrices, 

Rotation_Indices  =>  Rotation_Indices, 

Rotation_Angles  =>  Rotation_Angles, 

Rotation_Angle_Vec  =>  Rotation_Angle_Vec) ; 

function  Align_CNE_Matrix  is  new 

DCM_GEN_PKG . Aligned_DCM_Matrix 

(Row_Indices  =>  Earth_Axes, 

Col_Indices  =■>  Navigation_Axes, 

Sin_Cos_Ratio  .  =*>  SinCosJRatio, 

Direction_Cosine_Matrix  =>  CNE_Matrices, 

Rotation_Indices  =>  Rotation_Indices, 

Rotation_Angles  =>  Rotation_Angles, 

Rotation_Angle_Vec  =>  Rotation_Angle_Vec) ; 


- routines  acting  as  interfaces  to  instantiated  routines 


- [Frame  Misalignment] - 

function  Frame_Misalignraent_of_GNE 

(CNE  :  CNE_Matricesj 

REF_DC_Matrix  :  CNE_Ma trices) 
return  Rotation_Angle_Vec  is 

begin 

return  Computed_Frame  Misalignment  (CNE,  REF_DC_Matrix) ; 

end  Frame_Misalignment_oI_CNE; 

- [Align  CNE  Matrix] - 

function  Aligned_CNE_Matrix 

(CNE  :  CNE_Matrices; 

Rotation_ar.>gle  :  Rotation_Angle_Vec) 
return  CNE_Matrices  is 

begin 

return  Align_CNE_Matrix  (CNE,  Rotation_Angle) ; 

end  Aligned_CNE_Matrix; 
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end  Alignment  Parts; 


3. 3. 2. 4. 9. 2. 9. 2. 7  UTILIZATION  OF  OTHER  ELEMENTS 
UTILIZATION  OF  OTHER  ELEMENTS  IN  TOP  LEVEL  COMPONENT: 


The  following  tables  describe  the  elements  used  by  this  part  but  defined 
elsewhere  in  the  parent  top  level  component: 


Packages: 

The  following  table  summarizes  the  external  packages  required  by  this  part: 


|  Name 

1  Type 

|  Source  | 

Description  | 

|DCM  General 
|  _Operations 

1 

| package 

1 

1 

1 

|  Direction 
j  _Cosine 
j  _Matrix 
j  _Operations 

|  This  package  contains  general  | 

|  operations  for  direction  cosine  j 
!  matrices.  Alignment  Parts  j 
!  instantiates  functions  and  procedures! 
j  from  this  package.  j 

Data  types: 

The  following  table  summarizes  the  types  required  by  this  part  and  defined 
elsewhere  in  the  parent  top  level  component: 

|  Name  | 

Type  | 

Source  | 

Description  | 

|  CNE  | 

i  Matrices! 

1  “  1 

1  1 

3X3  | 

matrixj 

1 

1 

CNE  j 

Operations! 

1 

r 

Type  representing  a  direction  cosine  j 

matrix  indexed  by  the  column  and  row  j 
types  representing  the  navigation  and  j 
earth  frame.  j 

3. 3. 2. 4. 9. 2. 9. 2. 8  LIMITATIONS 
None. 


3. 3. 2. 4. 9. 2. 9. 2. 9  LLCSC  DESIGN 
None. 


3.3.2.4.9.2.9.2.10  UNIT  DESIGN 
None. 


i 

>'  ! 


**r- 


1  X*  Xa  --'V  **-*  r-a. 


CAMP  Software  Detailed  Design  Document 


Page 


3. 3. 2. 4. 9. 2. 9. 3  CNE_FROM_QUATERNION  PACKAGE  DESIGN  (CATALOG  #P300-0) 

This  package  contains  a  function  which  initializes  the  CNE  matrix  using  a 
quaternion. 

The  decomposition  for  this  part  is  the  same  as  that  shown  in  the  Top-Level 
Design  Document. 

3. 3. 2. 4. 9. 2. 9. 3.1  REQUIREMENTS  ALLOCATION 

The  following  table  summarizes  the  allocation  of  CAMP  requirements  to  this 
part: 


|  Name 

|  Requirements  Allocation  | 

|  CNE  From_Quaternion 

|  N/A  | 

3. 3. 2. 4. 9. 2. 9. 3. 2  LOCAL  ENTITIES  DESIGN 
Subprograms : 

The  function  ,,Computed_CNE',  is  created  and  maintained  by  this  part. 

3. 3. 2. 4. 9. 2. 9. 3. 3  INPUT/OUTPUT 
GENERIC  PARAMETERS: 

Data  types: 

The  following  table  summarizes  the  generic  formal  types  required  by  this  part: 


|  Name 

1  Type 

1 

Description 

1 

|  Quaternion 

|  discrete 

1 

Data  type  which  translates  into  the 

i  Indices 

1  type 

1 

indexing  elements  which  index  the 

1 

1 

quaternion. 

j  Quaternion 

|  1X4 

1 

Type  representing  the  unit  quaternion  which 

|  _Vector 

j  matrix 

1 

rotates  the  XYZ  frame  into  the  xyz  frame. 

Data  objects: 

The  following  table  summarizes  the  generic  formal  objects  required  by  this 
part: 
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|  Name 


|  Value  |  Description 


Quaternion 

Indices 


Quaternion 

Indices 


Quaternion 

Indices 


Quaternion 

Indices 


’  FIRST 


SUCC 
' FIRST 


PRED 

'LAST 


Object  to  be  used  as  an  index  representing 
the  first  element  of  the  quaternion. 


Object  to  be  used  as  an  index  representing 
the  second  element  of  the  quaternion. 


Object  to  be  used  as  an  index  representing 
the  third  element  of  the  quaternion. 


Object  to  be  used  as  an  index  representing 
the  fourth  element  of  the  quaternion. 


FORMAL  PARAMETERS; 


The  following  describes  the  formal  parameters  for  the  function  "Compute_CNE": 


|  Name 


I  Type 


|  Mode  |  Description 


|  Quaternion  |  Quaternion  |  In  |  This  object  represents  the  unit 
j  j  _Vectors  j  j  quaternion  which  rotates  XYZ  into  xyz. 


3. 3. 2. 4. 9. 2. 9. 3. 4  LOCAL  DATA 


None. 


3. 3. 2. 4. 9. 2. 9. 3. 5  PROCESS  CONTROL 


Not  applicable. 


3. 3. 2. 4. 9. 2. 9. 3. 6  PROCESSING 


The  following  describes  the  processing  performed  by  this  part; 


separate  (Direction_Cosine_Matrix  Operations. CNE_Operations) 
package  body  CNE_From_Qua tern ion  Is 


—  — local  instantiations 


— instantiation  of  a  unit  from  "DCM_General_Operations"  with  CNE 
—  specific  parameters  applied  to  tne  generics> — 


function  Computed_CNE  is  new 

DCM_GEN_PKG . DCM_From_Quaternion 

(Row_Indices  =>  Earth_Axes, 

Col_Indices  =>  Navigation_Axes, 

Sin  Cos  Ratio  =>  Sin  Cos  Ratio, 
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Direction_Cosine_Matrix  =>  CNE_Matrices, 
Quaternion_Indices  =>  Quaternion_Indices, 

Quaternion  Vectors  =>  Quaternion  Vectors); 


- function  bodies 


- <the  function  to  call  the  above  function> - 

function  Corapute_CNE 

(quaternion  :  Quaternion_Vectors) 
return  CNE_Matrices  is 

begin 

return  Coraputed_CNE  (quaternion); 
end  Compute_CNE; 

end  CNE  From  Quaternion; 


3. 3. 2. 4. 9. 2. 9. 3. 7  UTILIZATION  OF  OTHER  ELEMENTS 

UTILIZATION  OF  OTHER  ELEMENTS  IN  TOP  LEVEL  COMPONENT: 

The  following  tables  describe  the  elements  used  by  this  part  but  defined 
elsewhere  in  the  parent  top  level  component: 

Packages : 

The  following  table  summarizes  the  external  packages  required  by  this  part: 


|  Name 

Type 

|  Source 

|  Description  | 

| DCM_General 
|  _Operations 

1 

| package 

1 

1 

|  Direction  |  This  package  contains  general  | 
j  _Cosine  j  operations  for  direction  cosine  j 
j  _Matrix  j  matrices.  CNE_From_Quaternion  j 
j  _Operationsj  instantiates  functions  and  procedures! 
i  ~  |  from  this  package.  j 

Data  types: 

The  following 
elsewhere  in 

table  summarizes  the  types  required  by  this  part  and  defined 
the  parent  top  level  component: 

|  Name  j 

Type  | 

Source  | 

Description  #  | 

|  CNE  | 

j  Matrices! 

i  "  1 

1  1 

3X3  | 

matrix] 

1 

1 

CNE  j 

Operations  j 

i 

1 

Type  representing  a  direction  cosine  { 

matrix  indexed  by  the  column  and  row  j 
types  representing  the  navigation  and  j 
earth  frame.  j 

1 
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3. 3. 2. 4. 9. 2. 9. 3. 8  LIMITATIONS 


None. 


3. 3. 2. 4. 9. 2. 9. 3. 9  LLCSC  DESIGN 


None. 


3.3.2.4.9.2.9.3.10  UNIT  DESIGN 
None. 


3.3.2.4.9.2.10  UNIT  DESIGN 

3.3.2.4.9.2.10.1  CNE_INITIALIZED_FR0M_EARTH_P0SITI0N  UNIT  DESIGN  (CATALOG  #P297-0) 

This  generic  function  initializes  a  CNE  matrix  from  the  earth  position.  Earth 
position  is  defined  through  the  wander  angle,  latitude,  and  longitude. 


3.3.2.4.9.2.10.1.1  REQUIREMENTS  ALLOCATION 
N/A 

3.3.2.4.9.2.10.1.2  LOCAL  ENTITIES  DESIGN 
None. 


3.3.2.4.9.2.10.1.3  INPUT/OUTPUT 


GENERIC  PARAMETERS: 

Data  types: 

The  following  table  describes  the  generic  formal  types  required  by  this  part: 


|  Name 

Type 

Description 

1 

Earth 

floating 

Type  which  translates  into 

the 

type  of 

1 

_Position 

point 

lattitude  and  longitude. 

1 

l 

Angles 

floating 

Type  which  translates  into 

the 

type  of  the 

1 

1 

point 

wander  angle. 

1 

Subprograms: 

The  following  table  describes  the  generic  formal  subroutines  required  by  this 
part: 
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j  Name  |  Type  |  Description 


Sin_Cos  |  procedure  |  This  procedure  computes  the  sine  and  cosine  of  an 

|  ]  object  of  type  Angles.  The  returned  values  are  of 

]  j  type  Sin_Cos_Ratio. 

Sin_Cos  j  procedure  j  This  procedure  computes  the  sine  and  cosine  of  an 

j  j  object  of  type  Earth_Position.  The  returned  values 

j  j  are  of  type  Sin_Cos_Ratio. 


FORMAL  PARAMETERS: 

The  following  table  describes  this  part's  formal  parameters: 


|  Name 

Type 

|  Mode 

Description 

|  Wander  Angle 

Angles 

1  In 

This  object  represents  the  wander  angle 

1 

which  is  the  angle  between  the  East  axi 

1 

of  the  navigation  frame  and  the  current 

i 

missile  direction  of  motion. 

|  Latitude 

Earth 

i 

1  In 

This  object  represents  the  current 

Position  1 

latitude  of  the  missile's  earth 

1 

location. 

j  Longitude 

Earth 

1 

1  In 

This  object  represents  the  current 

Position 

longitude  of  the  missile's  earth 

1 

location. 

3.3.2.4.9.2.10.1.4  LOCAL  DATA 
Data  objects: 

The  following  table  describes  the  data  objects  maintained  by  this  part: 
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1 

Name 

1  Type 

|  Value  | 

Description 

1 

1 

Sin  Lat 

jSin  Cos 

|sine  of  j 

This  object  stores  the  value 

of 

the 

1 

1 

1 

|  _Ratio 

j  latitude  j 

sine  of  earth  latitude. 

1 

Cos  Lat 

|Sin  Cos 

| cosine  of  | 

This  object  stores  the  value 

of 

the 

1 

1 

|  _Ratio 

j latitude  j 

1  I 

cosine  of  earth  latitude. 

! 

1 

1 

1 

Sin  Long 

1 

|Sin  Cos 

1  1 
|sine  of  j 

This  object  stores  the  value 

of 

the 

1 

1 

1 

1 

|  _Ratio 

j  longitude] 

1  1 

sine  of  earth  longitude. 

1 

i 

i 

1 

Cos  Long 

|Sin  Cos 

|sine  of  j 

This  object  stores  the  value 

of 

the 

i 

1 

]  Ratio 

j  longitudej 
j  1 

cosine  of  earth  longitude. 

1 

1 

1 

Sin  Wa 

|Sin  Cos 

1  1 
|sine  of  j 

This  object  stores  the  value 

of 

the 

1 

1 

j  Ratio 

j  wander  j 

sine  of  the  wander  angle. 

1 

1 

1 

1 

1  angle  j 

I  ! 

1 

1 

1 

Cos  Wa 

1 

|Sin  Cos 

| cosine  of  | 

This  object  stores  the  value 

of 

the 

1 

1 

|  Ratio 

j  wander  j 

cosine  of  the  wander  angle. 

1 

| 

j  angle  j 

1  t 

1 

i 

1 

1 

CNE 

1  CNE 

1  i 

|  N.A.  1 

This  object  is  a  3X3  direction  cosine 

i 

1 

1 

|  Matrices |  j 

matrix.  This  object  is  computed 

1 

1 

1 

1  1 

and  returned  by  this  function. 

1 

3.3.2.4.9.2.10.1.5  PROCESS  CONTROL 
Not  applicable. 


3.3.2.4.9.2.10.1.6  PROCESSING 

The  following  describes  the  processing  performed  by  this  part: 

separate  (Direction  Cosine_Matrix_Operations.CNE_Op.?rations) 
function  CNE_InitiaIized_From_Earth_Position 
(Wander  Angle  :  Angles; 

Latitude  :  Earth_Position; 

Longitude  :  Earth_Position) 
return  CNE  Matrices  is 


—  — declaration  section 


- <objects  to  hold  the  sine  and  cosine  of  latitude,  longitude, 

—  —  and  wander  angle> — 


Sin_Lat 
Cos_Lat 
Sin_Long 
Cos_Long 
Sin  Wa 


Sin_Cos_Ratio; 
Sin_Cos_Ratio; 
Sin_Cos_Ratio; 
Sin_Cos_Ratio; 
Sin  Cos  Ratio; 


,u  B  -■  .  -•  .J—r  -■  -•  --  s  1-  -  S  •-  \~~-,rz~rTV 
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Cos_Va  :  Sin_CosJRatio; 

—  - <the  CNE  to  be  returned> - 

CNE  :  CNE_Matrices; 

— begin  function 

begin 

- <compute  sines  and  cosines> — 

Sin_Cos( Latitude,  Sin_Lat,  Cos_Lat); 

Sin_Cos( Longitude,  Sin_Long,  Cos_Long) ; 

Sin_Cos(Wander_Angle,  Sin_Wa,  Cos_Va); 

- <compute  CNE  elements> — 

CNE (Greenw, East)  :=  -Cos_Wa  *  Sin_Long  -  Sin_Va  *  Sin_Lat  *  Cos_Long; 

CNE(Right,  East)  :=  Cos_Va  *  Cos_Long  -  Sin_Wa  *  Sin_Lat  *  Sin~Long; 

CNE( Polar,  East)  :=  Sin_Wa  *  Cos_Lat; 

.  CNE(Greenw, North)  :=  Sin_Wa  *  Sin_Long  -  Cos_Wa  *  Sin_Lat  *  Cos_Long; 

CNE(Right,  North)  :*  -Sin_Va  *  Cos_Long  -  Cos_Va  *  Sin_Lat  *  Sin-Long; 

CNE(Polar,  North)  :=  Cos_Wa  *  Cos_Lat; 

CNE  (Greenw,  Up)  CosLat  *  Cos_Long; 

CNE(Right ,  Up)  :*  Cos_Lat  *  Sin  Long? 

CNE( Polar,  Up)  Sin_Lat; 

return  CNE; 

end  CNE_Initialized_From_Earth_Position; 

3.3.2.4.9.2.10.1.7  UTILIZATION  OF  OTHER  ELEMENTS 
None. 

3.3.2.4.9.2.10.1.8  LIMITATIONS 


None. 
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package  body  Direction_Cosine_Matrix_Operations  is 
package  body  Dcm_General_Operations  is  separate; 
package  body  Cne_Operations  is  separate; 
end  Direction_Cosine_Matrix_Operations ; 
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separate  (Direction_Cosine_Matrix_Operations) 
package  body  Dcm_General_Operations  is 

function  Dcm_Initialized_From_Reference 

(Ref_Dcm_2_l  :  Sin_Cos_Ratio; 

Ref_Dcm_2_2  :  Sin_Cos_Ratio; 

Ref_Dcm_3_l  :  Sin_Cos_Ratio; 

Ref_Dcm_3_2  :  Sin_Cos_Ratio; 

Sign_0f_2_3  :  INTEGER; 

Sign_0f_3_3  :  INTEGER) 

return  Direction_Cosine_Matrix  is  separate; 

package  body  Dcm_Trapezoidal_Integration  is  separate; 

procedure  Perform_Rectangular_Integration_Of_Dcm 

(Dc_Matrix  :  in  out  Direction_Cosine_Matrix; 

X_Rho  :  in  Angular_Velocities; 

Y_Rho  :  in  Angular_Velocities; 

Delta_Time  :  in  Time_Intervals)  is  separate; 

procedure  Reorthonormalize_Dcm 

(Dcm_Matrix  :  in  out  Direction_Cosine_Matrix)  is  separate; 

function  Frarae_Misalignment 

(Dcm_Matrix  :  Direction_Cosine_Matrix; 

Ref_Dc_Matrix  s  Direction_Cosine_Matrix) 
return  RotationAngleVec  is  separate; 

function  Aligned_Dcm_Matrix 

(Dcm  Matrix  s  Direction_Cosine_Matrix; 

Rotation_Angle  :  Rotation_Angle_Vec) 
return  DirectionCosineMatrix  is  separate; 

procedure  Compute_Firs t_Row_From_Orthonormal 

(Dcm_Matrix  :  in  out  Direction_Cosine_Matrix)  is  separate; 

function  Dcm_From_Quaternion 

(Quaternion  :  Quaternion_Vectors) 

return  Direction_Cosine_Matrix  is  separate; 


end  Dcm_General_Operations; 
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separate  (Direct ion_Cosine_Matrix_Operations.Dcm_General_Operations) 
function  Dcm_Initiali2ed_From_Reference 

(Ref  Dcm  2  1  :  Sin  Cos  Ratio; 


Ref_Dcm_2_2 
Ref_Dcm_3_l 
Ref_Dcm_3_2 
Sign_0f_2_3 
Sign  Of  3  3 


:  Sin_Cos_Ratio; 
:  Sin_Cos_Ratio; 
:  Sin_Cos_Ratio; 
:  INTEGER; 

:  INTEGER) 


return  Direction  Cosine  Matrix  is 


—  —  <  direct  ion  cosine  matrix  to  be  computed> — 

Dcm_Matrix  :  Direction_Cosine_Matrix; 

begin 

—  - <  compute  new  DCM> — 

if  Sign_0f_2_3  =  1  then 

Dcm_Matrix(Row2,Col3)  :=  Sqrt  (1.0  -  (  Ref_Dcm_2_l  * 

Ref_Dcm_2_l 

+ 

Ref_Dcra_2_2  * 
Ref  Dcm  22  ) 


i  ^ 

I 


Dcm_Matrix(Row2,Col3)  :=  -  Sqrt  (1.0  -  (  Ref_Dcm_2_l  * 

Ref"~Dcm  2  1 


Ref_Dcm_2_2  * 
Ref  Dcm  22  ) 


end  if; 


if  Sign_0f_3_3  *  1  then 

Dcm_Matrix(Row3,Col3)  :=  Sqrt  (1.0  -  (  Ref_Dcm_3_l  * 

Ref_Dcm_3_l 

+ 

Ref_Dcm_3_2  * 
Ref  Dcm  3  2  ) 


Dcm__Matrix(Row3,Col3)  :=  -  Sqrt  (1.0  -  (  Ref_Dcm_3_l  * 

Ref_Dcm_3_l 

+ 

Ref_Dcm_3_2  * 
Ref  Dcm  32  ) 


end  if; 

Dcm  Matrix(Rovl,Coll) 


Ref_Dcm_2_2  * 
Dcm_Matrix(Rov3,Col3) 

Dcm  Matrix(Row2,Col3)  * 


ms*  >>  .’v  w’- 
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Ref  Dcm  3  2 5 


Dcm  Matrix(Rowl,Col2) 


Dcm  Matrix(Rowl,Col3) 


Dcm_Matrix(Row2 , Coll ) 
Dcm_Matrix(Rov2,Col2) 
Dcm_Matrix(Row3 , Coll ) 
Dcm_Matrix(Row3 , Col2 ) 

return  Dcm_Matrix; 

end  Dcm  Initialized  From 


:=  Dcm_Matrix(Row2,Col3) 
Ref_Dcm_3_l 

Ref_Dcm_2_l  * 
Dcm_Matrix(Row3 , Col3) 

:=  Ref_Dcm_2_l  * 
Ref_Dcm_3_2 

Ref_Dcm_2_2  * 
Ref_Dcm_3_l; 

:=  Ref_Dcm_2_l; 

:=  Ref_Dcm_2_2; 

:=  Ref_Dcm_3_l; 

:=  Ref  Dcm  3  2; 


Reference; 
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separate  (Direction_Cosine_Matrix_Operations.Dcm_General_Operations) 
package  body  Dcm_Trapezoidal_Integration  is 

Prev_X_Rho  :  Angular_Velocities  :=  Initial_X_Rho; 

Prev_Y_Rho  :  Angular_Velocities  :=  Initial_Y_Rho; 

procedure  Reinitialize_Angular_Velocities 

(X  Rho  :  in  Angular_Velocities; 

Y^Rho  :  in  Angular_Velocities)  is  separate; 

procedure  Perform_Trapezoidal_Integration_Of_Dcra 

(Dc  Matrix  :  in  out  Direction  Cosine  Matrix; 


X  Rho 

:  in 

Angular_Veloci ties ; 

Y_Rho 

:  in 

Angular  Velocities; 

Delta_Time 

:  in 

Time_Intervals)  is  separate; 

end  Dcm_Trapezoidal_Integration; 


I 


1 


’i 


j 


'i 
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separate  (Direction_Cosine_Matrix_Operations.Dcm_General_Operations. 

Dcm_Trapezoidal_Integration) 
procedure  Reinitialize_Angular_Velocities 

(X_Rho  :  in  Angular_Velocities; 

Y_Rho  :  in  Angular_Velocities)  is 

begin 

Prev_X_Rho  s=  X_Rho ; 

Prev_Y_Rho  :=  Y_Rho; 

end  Reinitialize_Angular_Velocities ; 


_ 
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separate  (Direct ion_Cosine_Hatrix_Operations.Dcm_General_Operations. 

Dcm_Trapezoidal_Integration) 
procedure  Perform_Trapezoidal_Integration_Of_Dcm 

(Dc_Matrix  :  in  out  Direction_Cosine_Matrix; 

X_Rho  :  in  Angular_Velocities; 

Y_Rho  :  in  Angular_Velocities; 

Delta  Time  :  in  Time  Intervals)  is 


— - <  objects  for  internal  calculations> — 


Prev_Dcm  :  Direction_Cosine_Matrix; 
X_Bound_Sum  :  Angular_Velocities; 
Y_Bound_Sum  :  Angular_Velocities; 

Del  T  Div  2  :  Time  Intervals; 


— begin  procedure 


begin 

—  - <copy  last  two  rows  of  DCM  into  previous  DCM  before  computing> - 

Prev_Dcm(Row2,Coll)  :=  Dc_Matrix(Rov2,Coll); 

Prev_Dcm(Rov2fCol2)  :  =  Dc_Matrix(Row2,Col2); 

Prev_Dcm(Row2,Col3)  :*  Dc_Matrix(Rov2,Col3); 

Prev_Dcm(Row3,Coll)  5=  Dc_Matrix(Rov3,Ccll) ; 

Prev_Dcm(Rov3,Col2)  :=«  Dc_Matrix(Row3,Col2); 

Prev_Dcm(Row3,Col3)  :*  Dc_Matrix(Row3,Col3) ; 

—  - <  compute  sum  of  X  and  Y  bounds  from  old  and  new  angular  velocity  > — 

X_Bound_Sum  :=  (X_Rho  +  Prev_X_Rho); 

Y_Bound_Sum  :=  (Y_Rho  +  Prev_Y_Rho); 

—  - < compute  delta  time  divided  by  2  -  for  algorithm  simplification>— 

Del_T_Div_2  :=  Delta_Tirae  *  Time_Intervals(0.5); 

—  - <  compute  new  DCM> — 

Dc_Matrix(Row3,Coll)  :=  Prev_Dcm(Rov3,Coll)  - 

(  Y_Bound_Sum  *  Prev  Dcm(Row3,Col3) 

*  Del_T_Div_2) ; 

Dc_Matrix(Row3,Col2)  :=  Prev_Dcm(Row3,Col2)  + 

(  X_Bound_Sum  *  Prev  Dcm(Row3,Col3) 

*  Del_T_Div_2) ; 

Dc_Matrix(Row3,Col3)  :=  Prev_Dcm(Rov3,Col3)  + 

(  Y_Bound_Sum  *  Prev_Dcm(Rov3,Coll)  - 
X_Bound  Sum  *  Prev  Dcra(Rov3,Col2)  ) 
*  Del  T  Div  2; 
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Dc_Matrix(Rov2,Coll)  :=  Prev_Dcm(Row2,Coll)  - 

(  Y_Bound_Sum  *  Prev_Dcm(Row2,Col3) 

*  Del_T_Div_2) ; 

Dc_Matrix(Row2,Col2)  :=  Prev_Dcm(Row2,Col2)  + 

(  X_Bound_Sum  *  Prev_Dcm(Row2,Col3) 

*  Del_T_Div_2) ; 

Dc_Matrix(Row2,Col3)  :=  Prev_Dcm(Rov2,Col3)  + 

(  Y_Bound_Sum  *  Prev_Dcm(Rov2,Coll)  - 
X_Bound_Sum  *  Prev_Dcm(Rov2,Col2)  ) 
*  Del_T_Div_2 ; 

Prev_X_Rho  :=  X_Rho; 

Prev_Y_Rho  :=  Y_Rho; 

end  Perform_Trapezoidal_Integration_Of_Dcn; 
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separate  (Direction_Cosine_Matrix_Operations.Dcm_General_Operations) 
procedure  Perform_Rectangular_Integration_Of_Dcm 

(Dc_Matrix  :  in  out  Direction_Cosine_Matrix; 

X_Rho  :  in  Angular_Velocities; 

Y_Rho  :  in  AngularJ/elocities; 

Delta  Time  :  in  Time  Intervals)  is 


—  — declaration  section 


—  - cOCM  for  computing  increments  (temporary)  > — 

Dcm_Incr  :  Directicn_Cosine_Matrix; 

—  - <  objects  for  internal  calculation  > — 

Y_Rh°_Times_Delta_Time  :  Sin_Cos_Ratio  :=  Y_Rho  *  DeltaJTime; 
X_Rh°_Times_Delta  Time  :  Sin  Cos  Ratio  :=  X  Rho  *  Delta  Time; 


— begin  procedure 


begin 


- <compute  increments  for  bottom  two  rows>— 


Dcm_Incr(Rov2,Coll)  := 
Dcm_Incr(Rov2,Col2)  :» 
Dcm_Incr(Row2,Col3)  := 

Dcm_Incr(Row3,Coll)  ;= 
Dcm_Incr(Row3,Col2)  := 
Dcm  Incr(Row3,Col3)  := 


-DC_Matrix(Row2 , Col3) 
Dc~Matrix(Row2 , Col3 ) 
Dc_Matrix(Rov2 , Coll ) 
Dc_Matrix(Rov2 , Col2) 
-DC_Matrix(Row3 , Col3) 
Dc_Matrix(Rov3,Col3) 
Dc_Matrix(Row3 , Coll) 
Dc  Matrix(Rov3,Col2) 


*  Y_Rho_Times_Delta_Time; 

*  X_Rho_Times_Delta_Time; 

*  Y_Rho_Times_Delta_Time 

*  X_Rho_Times_Del ta_Time ; 

*  Y_Rho_Times_Delta_Time; 

*  X_Rho_Times_Delta_Time; 

*  Y_Rho_Times_Delta_Time 

*  X  Rho  Times  Delta  Time; 


-  —  <  compute  current  values  of  bottom  two  Rows>~ 


Dc_Matrix(Rov2 , Coll ) 
Dc_Mat rix(Row2 , Col2 ) 
Dc_Matrix(Rov2,Col3) 

Dc_Matrix(Rov3 , Coll ) 
Dc_Matrix(Rov3 , Col2) 
Dc_Matrix(Row3,Col3) 


Dc_Matrix(Row2,Coll) 
Dc_Matrix(Rov2 , Col2 ) 
Dc_Matrix(Row2,Col3) 

Dc_Matrix(Rov3 , Coll ) 
Dc_Matrix(Rov3,Col2) 
Dc  Matrix(Row3,Col3) 


+  Dcm_Incr(Row2,Coll); 
+  Dcm_Incr(Row2,Col2) ; 
+  Dcm_Incr(Row2,Col3); 

+  Dcm_Incr(Row3,Coll) ; 
+  Dcm_Incr(Row3,Col2) ; 
+  Dcm  Incr(Row3,Col3); 


end  Perform_Rectangular_Integration_Of_Dcra; 
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separate  (Direction_Cosine_Matrix_Operations.Dcm_General_Operations) 
procedure  Reorthonormalize_Dcm 

(Dcm_Matrix  :  in  out  Direction_Cosine_Matrix)  is 


—  — declaration  section 


■  <  objects  for  storing  dot  products  >- 


Rov2_Dot_Row2 
Rov3_Dot_Row3 
Row2  Dot  Row3 


Real; 

Real; 

Real; 


Half  Rov2  Dot  Rov3  :  Sin  Cos  Ratio; 


— begin  procedure 


begin 

Row2  Dot  Row2 


Dcm_Matrix(Row2,Coll)  *  Dcm_Matrix(Row2,Coll)  + 
Dcm_Matrix(Rov2,Col2)  *  Dcm_Matrix(Row2,Col2)  + 
Dcm_Matrix(Rov2,Col3)  *  Dcm_Matrix(Row2,Col3) ; 


Row3.Dot  Row3  := 


Dcm_Matrix(Row3,Coll)  *  Dcm_Matrix(Row3f Coll)  + 
Dcm_Matrix(Row3,Col2)  *  Dcm_Matrix(Row3,Col2)  + 
Dcm_Matrix(Rov3,Col3)  *  Dcm~Matrix(Rov3,Col3); 


—  — calculate  column  values 


Dcm  Matrix(Rov2,Coll)  ;  = 


Dcm  Matrix(Row2,Coll)  * 

(1.5  -  Real(0.5)  *  Rov2  Dot 


Dcm  Matrix(Row3,Coll)  := 


Dcm  Matrix(Rov3,Coll)  * 

(1.5  -  Real(0.5)  *  Row3  Dot 


Dcm  Matrix(Row2,Col2)  ;= 


Dcm  Matrix(Rov2,Col2)  * 

(1.5  -  Real(0.5)  *  Rov2  Dot 


Dcm  Matrix(Row3,Col2)  := 


Dcm  Matrix(Rov3,Col2)  * 
(1.5  -  Real(0.5)  *  Row3  Dot 


Dcm  Matrix(Row2,Col3)  := 


Dcm  Matrix(Row2,Col3)  * 

(1.5  -  Real(0.5)  *  Row2  Dot 


Dcm  Matrix(Row3,Col3)  := 


Dcm  Matrix(Row3,Col3)  * 

(1.5  -  Real(0.5)  *  Row3  Dot 


Row2); 
Row3); 
Rov2); 
Row3); 
Row2) ; 
Row3); 


—  —  <  compute  Rows  2  thru  3  > 


Row2_Dot_Rov3  :=  Dcm_Matrix(Row2,Coll)  *  Dcm_Matrix(Rov3,Coll)  + 
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Dcm_Matrix(Row2,Col2)  *  Dcm_Matrix(Row3,Col2)  + 
Dcm_Matrix(Row2,Col3)  *  Dcm_Matrix(Rov3,Col3) ; 

Half_Row2_Dot_Row3  :=  0.5  *  Row2_Do t_Row3 ; 

Dcm_Matrix(Row2,Coll)  :=  Dcm_Matrix(Rov2,Coll)  - 

(Half_Row2_Dot_Row3  *  Dcm_Matrix(Row3,Coll)); 

Dcm_Matrix(Row3,Coll)  :=  Dcm_Hatrix(Row3,Coll)  - 

(Half_Rov2_Dot_Rov3  *  Dcm_Matrix(Row2,Coll) ) ; 

Dcm_Matrix(Row2,Col2)  :=  Dcm  Hatrix(Row2,Col2)  - 

(HaTf_Row2_Dot_Row3  *  Dcm_Matrix(Row3,Col2)) ; 

Dcm_Matrix(Row3,Col2)  :=  Dcm  Matrix(Rov3,Col2)  - 

(HaIf_Row2_Dot_Row3  *  Dcm_Matrix(Row2,Col2)); 

Dcm_Matrix(Row2,C:>13)  :=  Dcm  Hatrix(Row2,Col3)  - 

(HaIf_Rov2_Dot_Rov3  *  Dcm_Hatrix(Row3,Col3)); 

Dcm_Matrix(Row3,Col3)  :=  Dcm  Matrix(Rov3,Col3)  - 

(HaIf_Rov2_Dot_Rov3  *  Dcm_Matrix(Row2,Col3) ) ; 

end  Reorthonormalize  Dcm; 
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separate  (Direction_Cosine_Matrix_Operations.Dcm_General_Operations) 
function  Frame_Misalignment 

(Dcm_Matrix  :  Direction_Cosine_Matrix; 
Ref_Dc_Matrix  :  Direction_Cosine_Matrix) 
return  Rotation_Angle_Vec  is 


—  — declaration  section 


— - <the  rotation  angle  vector  that  is  retumed> — 

Rot_Angle  :  Rotation_Angle_Vec; 


— begin  Junction 


begin 

Rot_Angle(X)  :=  Dcm_Matrix(Rovl,Col2)  *  Ref_Dc_Matrix(Rovl,Col3)  + 
Dcm_Matrix(Rov2,Col2)  *  Ref_Dc_Matrix(Rov2,Col3)  + 
Dcra_Matrix<Rov3,Col2)  *  Ref_Dc_Matrix(Row3,Col3) ; 

Rot_Angle(Y)  :=*  Dcm_Matrix(Rovl,Col3)  *  Ref _Dc_Matrix(Rowl, Coll)  + 
Dcm_Matrix(Rov2,Col3)  *  Ref JDc_Matrix(Rov2, Coll)  + 
Ucm_Matrix(Row3,Col3)  *  Ref_Dc_.Matrix(Rov3 , Coll ) ; 

Rot_Angle(Z)  :=  Dcm_Matrix(Rowl,Coll)  *  Ref_Dc_Matrix(Rovl,Col2)  + 
Dcm_Matrix(Row2,Coll)  *  Ref _Dc_Matrix(Rov2,Col2)  + 
Dcm_Matrix(Rov3,Coll)  *  Ref_Dc_Matrix(Rov3,Col2) ; 

return  Rot_Angle; 
end  Frame_Misalignment; 


I 

K 


i 
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separate  (Direction_Cosine_Matrix_Operations.Dcm_General_Operations) 
function  Aligned_Dcm_liatrix 

(Dcm_Matrix  :  Direction_Cosine_Matrix; 

Rotation_Angle  :  Rotation_Angle_Vec) 
return  Direction  Cosine  Matrix  is 


—  — declaration  section 


- <  computed  DCM  to  be  retumed> — 

Dcm  New  :  Direction  Cosine  Matrix; 


— begin  function 


begin 

Dcm_New(Row2,Coll)  := 
Dcm_New(Rov2,Col2)  :=* 
Dcm_Nev(Row2,Col3)  :* 

Dcm_New(Row3,Coll)  := 
Dcm_New(Row3 , Col2 )  := 
Dcm_New(Row3,Col3)  := 


Dcm_Matrix(Row2,Coll)  + 
Rotation_Angle(Y) ; 

Dcm_Matrix(Rov2,Col2)  - 
RotationAngle(X) ; 

Dcm_Matrix(Row2,Col3)  - 
Rotation  Angle(Y)  + 
Dcm_MatrIx(Rov2,Col2)  * 

Dcm_Matrix(Rov3,Coll)  + 
Rotation_Angle(Y) ; 

Dcm_Matrix(Row3,Col2)  - 
Rotation_Angle(X) ; 

Dcm_Matrix(Row3,Col3)  - 
Rotation  Angle(Y)  + 

Dcm  MatrIx(Row3,Col2)  * 


Dcm_Matrix(Row2,Col3)  * 

Dcm_Matr ix(Row2 , Col3 )  * 

Dcm_Matrix(Rov2,Coll)  * 
Rotation_Angle(X) ; 
Dcm_Matrix(Row3,Col3)  * 

Dcm_Matrix(Row3,Col3)  * 

Dcm_Matrix(Row3,Coll)  '•* 
Rotation_Angle(X) ; 


—  - <set  first  row  equal  to  original  matrix>  - 

—  - <NOTE:  The  first  row  is  now  INVALID  data>~ 

Dcm_New(Rowl,Coll)  :=  Dcm_Matrix(Rowl,Coll) ; 
Dcm_New(Rowl,Col2)  :=  Dcm_Matrix(Rowl,Col2) ; 
Dcm_New(Rowl,Col3)  :=  Dcm_Matrix(Rowl,Col3) ; 

return  Dcm_New; 

end  Aligned_Dcm_Matrix; 


i: 
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separate  (Direction_Cosine_Matrix_Operations.Dcm_General_Operations) 
procedure  Compute_Firs  t_Row_From_0r thonormal 


(Dcm  Matrix 


:  in  out  Direction  Cosine  Matrix)  is 


begin 


Dcm_Matrix(Rowl,Coll)  :=  Dcm_Matrix(Row2,Col2)  *  Dcm_Matrix(Row3,Col3) 

-  Dcm_Matrix(Row2,Col3)  *  Dcm_Matrix(Row3,Col2) ; 

Dcm_Matrix(Rovl,Col2)  :=  Dcm_Matrix(Row2,Col3)  *  Dcm_Matrix(Row3,Coll) 

-  Dcm  Matrix(Rov2,Coll)  *  Dcm  Matrix(Row3,Col3); 


Dcm  Matrix(Rowl,Col3)  := 


Dcm_Matrix(Rov2,Coll)  *  Dcm_Matrix(Row3,Col2) 

-  Dcm  Matrix(Rov2,Col2)  *  Dcm  Matrix(Row3,Coll) ; 


end  Compute  First  Row  From  Orthonormal; 


» 
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separate  (Direction_Cosine_Matrix_Operations.Dcm_General_Operations) 
function  Dcm_From_Quaternion 

(Quaternion  :  Quaternion_Vectors) 
return  Direction_Cosine_Matrix  is 


—  —  declaration  section 


—  - <  objects  to  hold  the  squares  of  quaternions  > — 

Q0_Sqr  :  Sin_Cos_Ratio; 

Ql_Sqr  :  Sin_Cos_Ratio; 

Q2_Sqr  :  Sin_Cos_Ratio; 

Q3_Sqr  :  Sin_Cos_Ratio; 

—  - <the  DCM  to  be  retumed> — 

Dcm  :  Direction  Cosine  Matrix; 


— begin  function 


begin 

- <  compute  squares  of  quaternions  > - 

QOSqr  :=  Quaternion(QO)  *  Quaternion(QO) ; 

QISqr  :=  Quaternion(Ql)  *  Quaternion(Ql) ; 

Q2_Sqr  :*  Quaternion(Q2)  *  Quaternion(Q2) ; 

Q3_Sqr  :»  Quaternion(Q3)  *  Quaternion(Q3) ; 


- <  compute  direction  cosines  > - 

Dcm(Rovl,Coll)  :=  Q0_Sqr  +  Ql_Sqr  -  Q2_Sqr  -  Q3_Sqr; 

Dcm(Row2,Col2)  :=  Q0_Sqr  -  Ql_Sqr  +  Q2_Sqr  -  Q3_Sqr; 

Dcm(Row3,Col3)  :=  Q0_Sqr  -  Ql_Sqr  -  Q2_Sqr  +  Q3_Sqr; 

Dcm(Row2,Coll)  :=  (  (Quaternion(QO)  *  Quaternion(Q3>) 

+(Quaternion(Ql)  *  Quaternion(Q2))  ) 

*  2.0; 

Dcra(Row3,Coll)  s=  (  (Quaternion(Ql)  *  Quaternion(Q3)) 

-(Quaternion(QO)  *  Quaternion(Q2))  ) 

*  2.0; 

Dcm(Rowl,Col2)  :=  (  (Quaternion(Ql)  *  Quaternion(Q2)) 

-(Quaternion(QO)  *  Quaternion(Q3))  ) 

*  2.0; 

Dcm(Row3,Col2)  :=  (  (Quaternion(QO)  *  Quaternion(Ql)) 

+(Quaternion(Q2)  *  Quaternion(Q3))  ) 

*  2.0; 

Dcm(Rowl,Col3)  :=  (  (Quaternion(QO)  *  Quaternion(Q2)) 
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+(Quaternion(Ql)  *  Quaternion(Q3))  ) 

*  2.0; 

Dcm(Row2,Col3)  :=  (  (Quaternion(Q2)  *  Quaternion(Q3)) 

-(Quaternion(QO)  *  Quaternion(Ql))  ) 

*  2.0; 


return  Dcm; 

end  Dcm  From  Quaternion; 


f 

,i 
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separate  (Direction_Cosine_Matrix_Operations) 
package  body  Cne_Operations  is 

package  Dcm_Gen_Pkg  renames 

Direction_Cosine_Matrix_Operations.Dcm_General_Operations; 


—  —  <  instantiation  of  a  units  from  "DCM  General  Operations"  with  CNE 

—  —  specif  c  parameters  applied  to  the  generics  > 


function  Initialize_From_Ref  is  new 

Dcm_Gen_Pkg. Dcm_Ini tialized_From_Reference 
(Rov_Indices  =>  Earth_Axes, 

Col_Indices  =>  Navigation_Axes, 

Sin_Cos_Ratio  =>  Sin_Cos_Ratio, 

Direction_Cosine_Matrix  =>  Cne_Matrices); 

procedure  Reorthonormalize  is  new 

Dcm__Gen_Pkg.Reorthonormalize_Dcm 

(Rov_Indices  =>  Earth_Axes, 

Col_Indices  =>  Navigation_Axes , 

Sin_Cos_Ratio  =>  Sin_Cos_Ratio, 

Direction_Ccsine_Matrix  =>  Cne  Matrices, 
Real  =>  Real); 

procedure  ComputeFirstRow  is  new 

Dcm  Gen_Pkg.Compute_First_Row_From_Orthonormal 
fRov_Indices  =>  Earth_Axes, 

Col_Indices  =>  Navigation_Axes, 

Sin_Cos_Ratio  =■>  Sin_Cos_Ratio, 

Direction_Cosine_Matrix  =>  Cne_Matrices); 


—  — separate  parts 


function  Cne_Initialized_From_Earth  Position 

^Wander  Angle  :  Angles; 

Latitude  :  Earth_Position; 
Longitude  :  Earth~Position) 
return  Cne_Matrices  Is  separate; 

package  body  Cne_Integration  is  separate; 

package  body  Alignment_Parts  is  separate; 

package  body  Cne_From_Quaternion  is  separate; 


—  —  routines  acting  as  interfaces  to  instantiated  routines 


function  Cne_Ini tialized_From_Ref erence 

(Ref_Cne_2_l_  :  Sin_Cos_Ratio; 
Ref~Cne_2_2  :  Sin_Cos__Ratio; 
Ref~Cne  3  1  :  Sin~Cos"Ratio; 


,  *  *  **  #  >  .  *•  **  <*  a  *  <■  **  v  *  tij*  v*  f 
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Ref_Cne_3_2  :  Sin_Cos_Ratio; 

Sign_0f_2_3  :  INTEGER; 

Sign_Of_3_3  :  INTEGER) 

return  Cne_Matrices  is 

begin 

return  Initialize_From_Ref 

(Ref_Dcm_2_l  =>  Ref_Cne_2_l, 

Ref_Dcm_2_2  =>  Ref_Cne_2_2 , 

Ref_Dcm_3_l  =>  Ref_Cne_3_l, 

Ref_Dcm_3_2  =>  Ref_Cne_3_2, 

Sign_0f_2_3  =>  Sign_0f_2_3, 

Sign_0f_3_3  =>  Sign_0f_3_3); 
end  Cne_Initialized_From_Reference; 

procedure  Reorthonormalize_Cne  (Cne  :  in  out  Cne_Matrices)  is 
begin 

Reorthonormalize  (Cne); 
end  Reorthonormalize_Cne; 

procedure  Compute  _First_Row_0f_Cne_From_0rthonormal 
(Cne  *:  in  out  Cne_Matrices)  is 

begin 

Compute_First_Row  (Cne); 
end  Compute_Firs t_Rov_0f_Cne_From_0r thonormal ; 

end  Cne_0perations; 


i 


2 
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separate  (Direction_Cosine_Matrix_Operations.Cne_Operations) 
function  Cne_Ini tialized_From_Ear th_Posi tion 
(Wander_Angle  :  Angles; 

Latitude  :  Earth_Position; 

Longitude  :  Ear th_Posi tion) 
return  Cne  Matrices  is 


—  — declaration  section 


—  - <  objects  to  hold  the  sine  and  cosine  of  latitude,  longitude, 

—  —  and  wander  angle  > — 

Sin_Lat  :  Sin_Cos_Ratio; 

Cos_Lat  :  Sin_Cos_Ratio; 

Sin_Long  :  Sin_Cos_Ratio; 

Cos_Long  :  Sin_Cos_Ratio; 

Sin_Wa  :  Sin_Cos_Ratio; 

Cos_Wa  :  Sin_Cos_Ratio; 

—  - <the  CNE  to  be  retumed> — 

Cne  s  Cne  Matrices; 


—  begin  function 


begin 

—  — <  compute  sines  and  cosines  >-- 

Sin_Cos(Latitude,  Sin_Lat,  Cos_Lat); 

Sin_Cos(Longitude,  Sin_Long,  Cos_Long); 

Sin_Cos(Wander_Angle,  Sin_Va,  Cos_Wa); 

—  — <  compute  CNE  elements  >~ 

Cne(Greenv,East)  :=  -C0s_Va  *  Sin_Long  -  Sin_Wa  *  Sin_Lat  *  Cos_I.ong; 

Cne(Right,  East)  :=  Cos~Wa  *  Cos_Long  -  Sin_Wa  *  Sin_Lat  *  Sin_Long; 

Cne (Polar,  East)  ;=  Sin_Wa  *  Cos_Lat; 

Cne(Greenw, North)  :=  Sin_Wa  *  Sin_Long  -  Cos_Wa  *  Sin_Lat  *  Cos_Long; 

Cne(Right,  North)  :=  -SIn_Wa  *  Cos_Long  -  Cos_Wa  *  Sin_Lat  *  Sin_Long; 

Cne(Polar,  North)  :=  Cos~Wa  *  Cos_Lat; 

Cne(Greenw,Up)  :=  Cos_Lat  *  Cos_Long; 

Cne(Right,  Up)  :=  Cos_Lat  *  Sin_Long; 

Cne(Polar,  Up)  :=  Sin_Lat; 

return  Cne; 

end  Cne  Initialized  From  Earth  Position; 
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separate  (Direction_Cosine_Matrix_Operations.Cne_Operations) 
package  body  Cne_Integration  is 


—  —  local  instantiations 


—  —  <  instantiation  of  a  units  from  "DCM  General  Operations "  with  CNE 

—  —  specific  parameters  applied  to  the  generics  > — 

package  Integrate_Trap  is  new 

Dcm_Gen  Pkg.Dcm_Trapezoidal_Integration 

(AnguIar_Velocities  =>  Angular_Velocities, 

Time_Intervals  =>  Time  Intervals, 

Row_Indicas  =>  EartE_Axes, 

Col_Indices  =>  Navigation_Axes, 

Sin_Cos_Ratio  =>  Sin_Cos_Ratio, 

Direction_Cosine_Matrix  =>  Cnejiatrices, 

Initial_X_Rho  ~  =>  Initial_East_Rho, 

Initial_Y_Rho  =>  Initial_North_Rho); 

procedure  Integrate_Rect  is  new 

Dcm_Gen  Pkg. Perf orm_Rectangular_Integration_Of_Dcm 
(AnguIar_Velocities  =>  Angular_Velocities, 

Time_Intervals  =>  Time  Intervals, 

Row_Indices  =>  EartE_Axes, 

Col_Indices  =*>  Navigation_Axes, 

Sin_Cos_Ratio  *>  Sin_Cos_Ratio, 

Direction_Cosine_Matrix  =>  Cne  Matrices); 


—  — routines  acting  as  interfaces  to  instantiated  routines 


- rf rapezoidal  Integration] - 

procedure  Perform_Trapezoidal_Integration_Of_Cne 

(Cne  ~s  in  out  Cne_Matrices; 

East  Rho  :  in  Angular_Velocities; 

NortH_Rho  :  in  Angular_Velocities; 

Delta_Time  :  in  Time_Intervals)  is 

begin 

Integrate_Trap.Perform_Trapezoidal_Integration_Of_Dcm 

(Cne,  East_Rho,  North_REo,  Delta_Time); 
end  Perform_Trapezoidal_Integration_Of_Cne; 


procedure  Reinit_Ang  Vel_For_Trapez_Integ_Of_Cne 

(East_REo  :  in  Angular_Velocities; 

NortE_Rho  :  in  Angular_Velocities)  is 

begin 

Integrate_Trap.Reinitialize_Angular_Velocities 

( Eas  t_Rho ,  Nor th_Rho ) ; 
end  Reini t_Ang_Vel_For_Trapez_Integ_Of_Cne ; 


/Rectangular  Integration ]■ 
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procedure  Perform_Rectangular_Integration_Of_Cne 
(Cne  :  in  out  Cne_Matrices; 

East_Rho  :  in  Angular_Velocities; 

North_Rho  :  in  Angular_Velocities; 

Delta_Time  :  in  Time_Intervals)  is 

begin 

Integrate_Rect  (Cne,  East_Rho,  North_Rho,  Delta_Time); 
end  Perform_Rectangular_Integration_Of_Cne; 


end  Cne_Integration; 
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separate  (Direction_Cosine_Matrix_Operations.Cne_Operations) 
package  body  Alignment_Parts  is 


—  — local  instantiations 


- <  instantiation  of  a  unit  from  "DCM  General Operations"  with  CNE 

specific  parameters  applied  to  the  generics  > — 

function  Computed_Frame_Misalignment  is  new 
Dcm_Gen_Pkg. Frame_Misalignment 

(Row_Indices  =>  Earth_Axes, 

Col_Indices  =>  Navigation_Axes, 

Sin_Cos_Ratio  =>  Sin_Cos_Ratio, 

Direction_Cosine_Matrix  =>  Cne_Matrices, 

Rotation_Indices  =>  Rotation_Indices, 

Rotation_Angles  =>  Rotation_Angles, 

Rotation_Angle_Vec  =>  Rotation_Angle_Vec) ; 

function  Align_Cne_Matrix  is  new 

Dcm_Gen_Pkg.Aligned_Dcm_Matrix 

(Row_Indices  =»>  Earth_Axes, 

Collndices  =>  Navigation_Axes, 

Sin_Cos_Ratio  =■>  Sin_Cos_Ratio, 

Direction_Cosine_Matrix  =>  Cne_Matrices, 

Rotationlndices  *>  Rotation_Indices, 

RotationAngles  =>  'RotationAngles, 

Rotation_Angle_Vec  *>  Rotation_Angle_Vec) ; 


—  —  routines  acting  as  interfaces  to  instantiated  routines 


—  - [Frame  Misalignment J - 

function  Frame_Misalignment_Of_Cne 

(Cne  :  Cne_Matrices; 

Ref_Dc_Matrix  :  Cne_Matrices) 
return  Rotation_Angle~Vec  is 

begin 

return  Computed_Frame  Misalignment  (Cne,  Ref_Dc_Matrix) ; 

end  Frame_Misalignment_Ol_Cne; 

—  - [Align  CNE  Matrix] - 

function  Aligned_Cne_Matrix 

(Cne  -  :  Cne_Matrices; 

Rotation_Angle  :  Rotation_Angle_Vec) 
return  Cne_Matrices  is 

begin 

return  Align_Cne_Matrix  (Cne,  Rotation_Angle) ; 

end  Aligned_Cne_Matrix; 

end  Alignment_Parts; 
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separate  (Direction_Cosine_Matrix_Operations.Cne_Operations) 
package  body  Cne_From_Quaternion  is 


—  —  local  instantiations 


—  —  <  instantiation  of  a  unit  from  "DCM  General  Operations"  with  CNE 

—  —  specific  parameters  applied  to  the  generics  > — 


function  Computed_Cne  is  nev 

Dcm  Gen  Pkg.Dcm  From  Quaternion 


(Row_Indices 
Col_Indices 
Sin_Cos_Ratio 
Direction_Cosine_Matrix 
Quaternion_Indices 
Quaternion  Vectors 


=>  Earth_Axes, 

=>  Navigation_Axes, 

=>  Sin_Cos_Ra"tio, 

=>  Cne_Matrices, 

=>  Quaternion_Indices, 
=>  Quaternion  Vectors); 


—  —  function  bodies 


— - <the  function  to  call  the  above  Junction  > — 

function  Compute_Cne 

(Quaternion  ;  Quaternion_Vectors) 
return  CneMatrices  is 

begin 

return  ComputedCne  (Quaternion); 
end  Compute_Cne; 

end  Cne_From_Quaternion; 
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3. 3. 3.1  KALMAN  FILTER  COMMON  PARTS  (BODY)  TLCSC  651  (CATALOG  #P163-0) 

This  part,  which  is  designed  as  an  Ada  package,  contains  specifications  for  all 
CAMP  parts  which  can  be  used  to  implement  a  Kalman  Filter  regardless  of  the 
type  of  H  matrix  used. 

The  decomposition  for  this  part  is  the  same  as  that  shown  in  the  Top-Level 
Design  Document. 

3. 3. 3. 1.1  REQUIREMENTS  ALLOCATION 

The  following  chart  summarizes  the  allocation  of  CAMP  requirements  to  this 
Tlcsc: 

|  Name  |  Requirements  Allocation  | 

|  State_Transition_And_Process_Noise_Matrices_  |  R145 
j  Manager  '  j 

|  Error_Covariance_Matrix_Manager  |  R146 

j  State_Transition_Matrix_Manger  j  R148 

3. 3. 3. 1.2  LOCAL  ENTITIES  DESIGN 
None. 

3. 3. 3. 1.3  INPUT/OUTPUT 
None. 

3. 3. 3. 1.4  LOCAL  DATA 
None. 

3. 3. 3. 1.5  PROCESS  CONTROL 
Not  applicable. 

3. 3. 3. 1.6  PROCESSING 

The  following  describes  the  processing  performed  by  this  part: 
package  body  Kalman_Filter_Common_Parts  is 

package  body  State_Transition_And_Process_Noise_Matrices_Manager  is  separate; 
package  body  Error_Covariance_Matrix_Manager  is  separate; 

package  body  State  Transition_Matrix_Manager  is  separate; 
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end  Kalman  Filter  Common  Parts; 


3. 3. 3. 1.7  UTILIZATION  OF  OTHER  ELEMENTS 
None. 


3.3.3. 1.8  LIMITATIONS 
None. 


3. 3. 3. 1.9  LLCSC  DESIGN 

3. 3. 3. 1.9.1  STATE  TRANSITION_AND_PROCESS_NOISE_MATRICES_MANAGER  PACKAGE  DESIGN 
(CATALOG  1P164-0) 

This  LLCSC  is  a  generic  package  which  manages  the  State  Transition  (Phi)  and 
Process  Noise  (Q)  matrices.  It  consists  of  an  Initialize  procedure,  a 
Propagation  function,  and  functions  which  return  the  stored  ■value  of  each  of 
the  two  matrices. 

The  decomposition  for  this  part  is  the  same  as  that  shown  in  the  Top-Level 
Design  Document. 


3. 3. 3. 1.9. 1.1  REQUIREMENTS  ALLOCATION 
This  part  meets  CAMP  requirement  R145. 


3. 3. 3. 1.9. 1.2  LOCAL  ENTITIES  DESIGN 
None. 


3. 3. 3. 1.9. 1.3  INPUT/OUTPUT 
GENERIC  PARAMETERS; 

Data  types: 

The  following  table  summarizes  the  generic  formal  types  required  by  this  part: 


|  Name 

|  Base  Type 

1 

Description 

1 

|  Time  Intervals 

|  floating  point 

1 

Type  for  the  delta  time  variable 

I  *Phi  Matrices 

j  private 

Data  type  of  N  x  N  Phi  Matrix 

I  Integrated  F 

j  private 

Data  type  of  N  x  N  Matrix  for 

|  Matrices 

i 

F  integration 

I  Integrated  Q 

|  private 

1 

Data  type  of  N  x  N  Matrix  for 

|  Matrices 

1 

1 

Q  integration 

j  Q_Ma trices 

|  private 

1 

Data  type  of  N  x  N  Q  Matrix 
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Subprograms : 

The  following  table  summarizes  the  generic  formal  subroutines  required  by  this 
part; 


|  Name 

| Add  To  Identity 


Set_To_Identity_ 

Matrix 

Set  To  Zero  Matrix 


ABA  Transpose 


I  Type  | 


Description 


procedure  Adds  the  identity  matrix  to  a 
Integrated_F_Matrices 

function  Sets  a  Phi_Matrices  type  matrix  to 
the  identity  matrix 

function  Sets  a  Q_Matrices  type  matrix  to  the 
zer  matrix 

function  Multiplies  a  Integrated_F_Matrices 
type  matrix  by  the  transpose  of  a 
Q_Matrices  type  matrix  yielding  a 
Q_Matrices  type  matrix 

function  Multiplies  a  Integrated  F  Matrix  by 

a  Time  Interval  yielding  a  Integrated 
Q  Matrix 

function  Multiplies  a  Integrated  F  Matrix  by  a 
Phi  Matrix  yielding  a  Phi  Matrix 

function  Adds  a  Integrated  Q  Matrix  to  a  Q 
Marix  yielding  a  Q  Matrix 


3. 3. 3. 1.9. 1.4  LOCAL  DATA 
Data  objects; 

The  following  table  describes  the  data  objects  maintained  by  this  part; 


|  Name 

1  Type  | 

Value  | 

Description 

|Prop  Phi 

| Statically  | 

variable | 

The  propagated  state  transition 

j  Sparse_  | 

j Matrices  \ 

matrix 

1  Prop  Q 

1  5 

[Symmetric  | 

1 

variable | 

The. propagated  process  noise 

j Matrices  j 

1 

matrix 

3. 3. 3. 1.9. 1.5  PROCESS  CONTROL 
Not  applicable. 


5 

A 
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3. 3. 3. 1.9. 1.6  PROCESSING 

The  following  describes  the  processing  performed  by  this  part: 
separate  (Kalman_Filter_Common_Parts) 

package  body  State_Transition_And_Process_Noise_Matrices_Manager  is 

Prop_Phi  :  Phi_Matrices; 

Prop_Q  :  Q_Matrices; 

end  State_Transi tion_And_Process_Noise_Matrices_Manager ; 


3. 3. 3. 1.9. 1.7  UTILIZATION  OF  OTHER  ELEMENTS 
None. 


3. 3. 3. 1.9. 1.8  LIMITATIONS 


None. 


3. 3. 3. 1.9. 1.9  LLCSC  DESIGN 


None. 


3.3.3.1.9.1.10  UNIT  DESIGN 

3.3.3.1.9.1.10.1  INITIALIZE  UNIT  DESIGN 

This  unit  initializes  the  Propagated  Phi  matrix  to  the  identity,  and  the 
Propagated  Q  matrix  to  the  zero  matrix. 

3.3.3.1.9.1.10.1.1  REQUIREMENTS  ALLOCATION 
This  part  meets  CAMP  requirement  R145 

3.3.3.1.9.1.10.1.2  LOCAL  ENTITIES  DESIGN 
None. 


3.3.3.1.9.1.10.1.3  INPUT/OUTPUT 
None. 

3.3.3.1.9.1.10.1.4  LOCAL  DATA 


None. 
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3.3.3.1.9.1.10.1.5  PROCESS  CONTROL 
Not  applicable. 

3.3.3.1.9.1.10.1.6  PROCESSING 

The  following  describes  the  processing  performed  by  this  part: 
procedure  Initialize  is 
begin 

Set_To_Identity_Matrix  (Source  =>>  Prop_Phi); 

Set_To_Zero_Matrix  (Source  =>  Prop_Q); 
end  Initialize; 

3.3.3.1.9.1.10.1.7  UTILIZATION  OF  OTHER  ELEMENTS 

UTILIZATION  OF  OTHER  ELEMENTS  IN  TOP  LEVEL  COMPONENT: 

The  following  tables  describe  the  elements  used  by  this  part  but  defined 
elsewhere  in  the  parent  top  level  component: 

Data  types: 

The  following  table  summarizes  the  types  required  by  this  part  and  defined  in 
the  parent  component: 


|  Name 

|  Base  Type 

|  Description 

1 

Phi  Matrices 

| private 

1 

1 

Data  type  of  matrix  used  to 
represent  Phi  matrices 

1 

1 

1 

Q  Matrices 

| private 

Data  type  of  matrix  used  to 

1 

1 

1 

represent  Q  matrices 

1 

1 

Data  objects: 

The  following  table  summarizes  the  objects  required  by  this  part  and  defined  in 
the  parent  component: 
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3.3.3.1.9.1.10.1.8  LIMITATIONS 
None. 

3.3.3.1.9.1.10.2  PROPAGATE  UNIT  DESIGN 

This  uni t  propagates  new  Propagated  Phi  and  Propagated  Q  matrices  given  a  new 
Integrated  System  Description  Matrix  and  Time  Interval 

3.3.3.1.9.1.10.2.1  REQUIREMENTS  ALLOCATION 
This  part  meets  CAMP  requirement  R145. 

3.3.3.1.9.1.10.2.2  LOCAL  ENTITIES  DESIGN 
None. 

3.3.3.1.9.1.10.2.3  INPUT/OUTPUT 
FORMAL  PARAMETERS: 

The  following  table  describes  this  unit's  formal  parameters: 


|  Name 

1  Type 

|  Mode 

|  Description  | 

|Integrated_F 

1 

|Integrated_ 

|F_Matrices~ 

1 

| 

|  in 

1 

1 

1 

Current  value  for  the  | 
System  Description  matrix  j 
already  integrated  across  j 
time  j 

|Q 

1 

i 

1 

|Integrated_ 

|Q_Matrices 

jin 

1 

1 

Current  process  noise  matrix J 

i 

i 

1 

IDT 

i 

1 

| Time_ 
j Intervals 

1 

|  in 

1 

1 

Time  since  the  last  | 

propagation  j 
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3.3.3.1.9.1.10.2.4  LOCAL  DATA 
Data  objects: 

The  following  table  describes  the  data  objects  maintained  by  this  part: 


|  Name 

1  Type  1 

Value  |  Description  | 

|  Phi 

1 

|Integrated_| 
|F  Matrices  j 
1  1 

variable |  Computed  process  noise  matrix  | 

j  matrix  j 

I  1 

3.3.3.1.9.1.10.2.5  PROCESS  CONTROL 
Not  applicable. 

3.3.3.1.9.1.10.2.6  PROCESSING 

The  following  describes  the  processing  performed  by  this  part: 

procedure  Propagate  (IntegratedJ?  :  in  Integrated_F_Matrices; 

Q  :  in  Integrated_Q  Matrices; 

DT  :  in  Time_IntervaTs)  is 

Phi  :  IntegratedFMatrices; 

begin 


—  Propagate  Phi 

Phi  :=  Add_To_Identity  (Source  Matrix  =>  Integrated_F) ; 
Prop_Phi  :=  Phi  *  Prop_Phi; 


—  Propagate  Q 

Prop_Q  :=  Q  *  DT  +  ABA_Transpose(  Phi,  Prop  Q  )  ; 
end  Propagate; 


1 


4 

1 

ij 

\ 

1 


H 


1 

J 

"i 

'i 


i 

i 

4 

3 

4 


i 


I 

% 


$ 

jj 


a 

I 


3.3.3.1.9.1.10.2.7  UTILIZATION  OF  OTHER  ELEMENTS 

UTILIZATION  OF  OTHER  ELEMENTS  IN  TOP  LEVEL  COMPONENT: 

The  following  tables  describe  the  elements  used  by  this  part  but  defined 
elsewhere  in  the  parent  top  level  component: 

Data  types: 


1 


•I 
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The  following  table  summarizes  the  generic  formal  types  required  by  this  part: 


|  Name 

|  Base  Type 

|  Description 

|Time  Intervals 

| floating 
j  point  type 

1 

|Type  for  the  delta 

1 

1 

time  variable 

|Phi_Ma trices 

1 

|  private 

l 

1 

(Data  type  of  N  x  N 

l 

Phi  Matrix 

j Integrated  F 
j Matrices 

1 

| private 

1 

i 

|Data  type  of  N  x  N 
|F  integration 

1 

Matrix  for 

j Integrated  Q 
j Matrices 

| private 

1 

1 

|Data  type  of  N  x  N 
|Q  integration 

1 

Matrix  for 

|Q_Matrices 

1 

| private 

|Data  type  of  N  x  N 

Q  Matrix 

Data  objects: 

The  following  table  summarizes  the  objects  required  by  this  part  and  defined  in 
the  parent  component: 


|  Name  |  Type  |  Value  |  Description 


|Prop  Phi 

Phi 

1 

variable |  The  propagated  state  transition 

1 

1  " 

1 

Matrices 

1 

| 

j  matrix 

1 

1 

|Prop  Q 

Q_Matrices 

1 

1 

i 

variable |  The  propagated  process  noise 

1 

1 

j  matrix 

1 

3.3.3.1.9.1.10.2.8  LIMITATIONS 
None. 

3.3.3.1.9.1.10.3  GET_CURRENT  UNIT  DESIGN 

This  unit  returns  the  current  value  of  the  Propagated  Phi  and  Propagated  Q 
matrices. 


3.3.3.1.9.1.10.3.1  REQUIREMENTS  ALLOCATION 
This  part  meets  CAMP  requirement  R145. 


3.3.3.1.9.1.10.3.2 

None. 


LOCAL  ENTITIES  DESIGN 
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3.3.3.1.9.1.10.3.3  INPUT/OUTPUT 
FORMAL  PARAMETERS: 

The  following  table  describes  this  unit's  formal  parameters: 


|  Name 

|  Type  |  Mode 

|  Description  | 

| Propagated  Phi 

| Phi  Matrices! out 

(Stored  Propagated  State  j 

1 

I 

1  ! 

(Transition  matrix  ! 

i  i 

1 

| Propagated  Q 

|Q  Matrices  |out 

3  1 

| Stored  Propagated  Process  j 

! 

1  1 

(Noise  Matrix  j 

3.3.3.1.9.1.10.3.4  LOCAL  DATA 
None. 

3.3.3.1.9.1.10.3.5  PROCESS  CONTROL 
Not  applicable. 

3.3.3.1.9.1.10.3.6  PROCESSING 

The  following  describes  the  processing  performed  by  this  part: 

procedure  Get_Current  (Propagated_Phi  :  out  Phi_Matrices} 

Propagated_Q  :  out  Q_Matrices  )  is 

begin 

Propagated_Phi  :=  Prop_Phi; 

Propagated_Q  :=  Prop_Q; 
end  Get_Current; 

3.3.3.1.9.1.10.3.7  UTILIZATION  OF  OTHER  ELEMENTS 
UTILIZATION  OF  OTHER  ELEMENTS  IN  TOP  LEVEL  COMPONENT: 

The  following  tables  describe  the  elements  used  by  this  part  but  defined 
elsevhere  in  the  parent  top  level  component: 

Data  types: 

The  following  table  summarizes  the  types  required  by  this  part  and  defined  in 
the  parent  component: 
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j  Name 

|  Base  Type 

j  Description  | 

| Phi  Matrices 

l  - 

| private 

l 

(Data  type  of  N  x  N  Phi  Matrix  | 

i  i 

|Q  Matrices 

1  " 

| private 

1 

|Data  type  of  N  x  N  Q  Matrix  j 

1  I 

Data  objects: 

The  following  table  summarizes  the  objects  required  by  this  part  and  defined  in 
the  parent  component: 


|  Name 

1  Type 

|  Value  |  Description  | 

|Prop  Phi 

1  " 
l 

|  Phi _ 

j  Matrices 

i 

|  variable |  The  propagated  state  transition  | 

i  j  matrix 

1  1 

|Prop  Q 

1  " 

|Q  Matrices 

1  " 

1  1  1 

|  variable |  The  propagated  process  noise  j 

j  j  matrix  j 

3.3.3.1.9.1.10.3.8  LIMITATIONS 
None. 

3.3.3.1.9.1.10.4  PROPAGATED_PHI  UNIT  DESIGN 

This  unit  returns  the  current  value  of  the  Propagated  Phi  matrix 

3.3.3.1.9.1.10.4.1  REQUIREMENTS  ALLOCATION 
This  part  meats  CAMP  requirement  R145. 

3.3.3.1.9.1.10.4.2  LOCAL  ENTITIES  DESIGN 
None. 

3.3.3.1.9.1.10.4.3  INPUT/OUTPUT 
FORMAL  PARAMETERS: 

The  following  table  describes  this  unit's  formal  parameters: 

|  Name  |  Type  |  Mode  |  Description 

|<returned  value>  |Phi_Matrices|out  | Stored  Propagated  State 
j  j  j  (Transition  matrix 
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3.3.3.1.9.1.10.4.4  LOCAL  DATA 
None. 

3.3.3.1.9.1.10.4.5  PROCESS  CONTROL 
Not  applicable. 

3.3.3.1.9.1.10.4.6  PROCESSING 

The  following  describes  the  processing  performed  by  this  part: 
function  Propagated_Phi  return  Phi_Matrices  is 
begin 

return  Prop_Phi; 
end  Propagated_Phi ; 

3.3.3.1.9.1.10.4.7  UTILIZATION  OP  OTHER  ELEMENTS 

UTILIZATION  OF  OTHER  ELEMENTS  IN  TOP  LEVEL  COMPONENT: 

The  following  tables  describe  the  elements  used  by  this  part  but  defined 
elsewhere  in  the  parent  top  level  component: 

Data  types: 

The  following  table  summarizes  the  types  required  by  this  part  and  defined  in 
the  parent  component: 


|  Name 

|  Base  Type 

|  Description  | 

|Phi_Matrices 

| private 

|Data  type  for  N  x  N  phi  matrix  | 

Data  objects: 

The  following  table  summarizes  the  objects  required  by  this  part  and  defined  in 
the  parent  component: 

|  Name  |  Type  |  Value  |  Description  | 

|Prop_Phi  |Phi_  I  variable |  The  propagated  state  transition  | 

j  [Matrices  j  j  matrix  j 
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3.3.3.1.9.1.10.4.8  LIMITATIONS 


None. 


3. 3. 3. 1.9. 2  ERROR_COVARIANCE_MATRIX_MANAGER  PACKAGE  DESIGN  (CATALOG  #P165-0) 

This  LLCSC  is  a  generic  package  which  manages  the  Error  Covariance  Matrix;  it 
consists  of  an  Initialize  procedure,  a  Propagation  procedure,  and  a  P  function, 
which  returns  the  current  Error  Covariance  matrix  value 

The  decomposition  for  this  part  is  the  same  as  that  shown  in  the  Top-Level 
Design  Document. 


3. 3. 3. 1.9. 2.1  REQUIREMENTS  ALLOCATION 
This  part  meets  CAMP  requirement  R146. 


3. 3. 3. 1.9. 2. 2  LOCAL  ENTITIES  DESIGN 


None. 


3. 3. 3. 1.9. 2. 3  INPUT/OUTPUT 
GENERIC  PARAMETERS: 

Data  types: 


The  following  table  summarizes  the  generic  formal  types  required  by  this  part: 


|  Name 

j  Base  Type 

|  Description  | 

1  Phi  Matrices 

l 

| private 

1 

|Data  type  of  N  x  N  Phi  matrix  | 

1  1 

} 

\ 

1 

|P  And  Q  Matrices 

1  "  "" 

| private 

1 

1  1 

(Data  type  of  N  x  N  P  and  Q  matrix | 

1  1 

3 

Subprograms : 

The  following  table 

summarizes 

the  generic  formal  subroutines  required  by  this 

part: 

- 

|  Name 

Type 

1 

Description  | 

i 

|ABA  Transpose 

function 

(Multiplies  a  Phi  matrix  by  the  j 

i 

jby  the  transpose  of  a  P  and  Q  matrix  j 

i 

j yielding  a  P  and  Q  matrix  j 

1 

j "+" 

function 

!  I 

|Adds  two  P  and  Q  matrices  yielding  [ 

©I 

1 

|a 

P  and  Q  matrix  | 

.3 

i 

i 

X 

j 


CAMP  Software  Detailed  Design  Document  Page  325 

wiy 


3. 3. 3. 1.9. 2. 4  LOCAL  DATA 

The  following  table  describes  the  data  objects  maintained  by  this  part: 


|  Name 

1  Type 

j  Value  |  Description  | 

| Stored  P 

1 

| P_And_Q_ 
j Matrices 

|  variable |  The  propagated  error  covariance  | 
j  j  matrix  which  is  stored  by  part  | 

3. 3. 3. 1.9. 2. 5  PROCESS  CONTROL 
Not  applicable. 

3. 3. 3. 1.9. 2. 6  PROCESSING 

The  following  describes  the  processing  performed  by  this  part: 
separate  (Kalman_Filter_Common_Parts) 
package  body  Error_Covariance_Matrix_Manager  is 
Stored_P  :  P_And_Q_Matrices; 

Vf>  end  Error_Covariance_Matrix_Manager; 

3. 3. 3. 1.9. 2. 7  UTILIZATION  OF  OTHER  ELEMENTS 
None. 

3. 3. 3. 1. 9. 2. 8  LIMITATIONS 
None. 


3. 3. 3. 1.9. 2. 9  LLCSC  DESIGN 
None. 

3.3.3.1.9.2.10  UNIT  DESIGN 
3.3.3.1.9.2.10.1  INITIALIZE  UNIT  DESIGN 

This  unit  initializes  the  Error  Covariance  matrix  to  the  matrix  sent  in. 


3.3.3.1.9.2.10.1.1  REQUIREMENTS  ALLOCATION 
This  part  meets  CAMP  requirement  R146 


MS® 


i»®' 
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3.3.3.1.9.2.10.1.2  LOCAL  ENTITIES  DESIGN 
None. 

3.3.3.1.9.2.10.1.3  INPUT/OUTPUT 
FORMAL  PARAMETERS: 

The  following  table  describes  this  unit's  formal  parameters: 


|  Name 

1  Type 

|  Mode  |  Description 

1 

| Initial  P 

IP  And  Q 

|  in  |This  is  the  value  to 

which  j 

; 

(Matrices 

!  j  the  Error  Covariance 

na  l1-l  a  j 

1 

1 

j  jwill  be  initialized 

1 

3.3.3.1.9.2.10.1.4  LOCAL  DATA 
None. 

3.3.3.1.9.2.10.1.5  PROCESS  CONTROL 
Not  applicable. 

3.3.3.1.9.2.10.1.6  PROCESSING 

The  following  describes  the  processing  performed  by  this  part: 
procedure  Initialize  (Initial_P  :  in  P_And_Q_Matrices)  is 
begin 

Stored_P  :=  Initial_P; 
end  Initialize; 

3.3.3.1.9.2.10.1.7  UTILIZATION  OF  OTHER  ELEMENTS 

UTILIZATION  OF  OTHER  ELEMENTS  IN  TOP  LEVEL  COMPONENT: 

The  following  tables  describe  the  elements  used  by  this. part  but  defined 
elsewhere  in  the  parent  top  level  component: 

Data  types: 

The  following  table  summarizes  the  types  required  by  this  part  and  defined  in 
the  parent  component: 
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|  Name 

j  Base  Type 

S  Description  | 

| P_And_Q_Ma trices 

(private 

jData  type  of  N  x  N  P  and  Q  matrix! 

Data  objects: 

The  following  table  summarizes  the  objects  required  by  this  part  and  defined  in 


the  parent 

component: 

|  Name 

1  Type 

(  Value  | 

Description  | 

(Stored  P 

IP  And  Q 

(  variable! 

The  propagated  error  covariance  | 

| 

1  Un  f 

J  UUW1.1V.V.O 

i  i 

t  i 

matrix  which  is  stored  by  part  j 

3.3.3.1.9.2.10.1.8  LIMITATIONS 
None. 

3.3.3.1.9.2.10.2  PROPAGATE  UNIT  DESIGN 

This  unit  propagates  the  Error  Covariance  Matrix  given  the  Propagated  Phi  and 
Propagated  Q  matrices 

3.3.3.1.9.2.10.2.1  REQUIREMENTS  ALLOCATION 
This  part  meets  CAMP  requirement  R146. 

3.3.3.1.9.2.10.2.2  LOCAL  ENTITIES  DESIGN 
None. 

3.3.3.1.9.2.10.2.3  INPUT/OUTPUT 
FORMAL  PARAMETERS: 

The  following  table  describes  this  unit's  formal  parameters: 


Name  |  Type  |  Mode  |  Description 


| Propagated  Phi 

| Phi  Matrices  in 

(Current  value  for  the  | 

| Propagated  State  Transition  | 

1 

| 

|Matrix  I 

1  1 

1 

(Propagated  Q 

IP  And  Q  in 

1  1 
| Current  value  for  the  1 

(Matrices 

j Propagated  Process  Noise  j 

1 

1 

j Matrix  j 

A, w\r o-.V'LV'f  v* <V»"k ‘V v\»  wv«ro f  O' *r O'  v."  (*,* 
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3.3.3.1.9.2.10.2.4  LOCAL  DATA 
None. 

3.3.3.1.9.2.10.2.5  PROCESS  CONTROL 
Not  applicable. 

3.3.3.1.9.2.10.2.6  PROCESSING 

The  following  describes  the  processing  performed  by  this  part: 

procedure  Propagate  (Propagated_Phi  :  in  Phi_Matrices; 

Propagated_Q  :  in  P_And_Q_Ma trices)  is 

begin 

Stored_P  :=  ABA_Transpose(  PropagatedJPhi,  Stored_P  )  +  Propagated_Q; 
end  Propagate; 

3.3.3.1.9.2.10.2.7  UTILIZATION  OF  OTHER  ELEMENTS 

UTILIZATION  OF  OTHER  ELEMENTS  IN  TOP  LEVEL  COMPONENT: 

The  following  tables  describe  the  elements  used  by  this  part  but  defined 
elsewhere  in  the  parent  top  level  component: 

Data  types: 

The  following  table  summarizes  the  types  required  by  this  part  and  defined  in 
the  parent  component: 


|  Name 

|  Base  Type 

|  Description 

1 

| Phi  Matrices 

1  “ 

| private 

1 

|Data  type  of  n 

I 

x  n  Phi  matrix  j 

1 

1 

1 P_And_Q_Ma  trices 

1 

1  private 

1 

|Data  type  of  n 

1 

x  n  P  and  Q  matrix] 

Data  objects: 

The  following  table  summarizes  the  objects  required  by  this  part  and  defined  in 
the  parent  component: 

|  Name  |  Type  |  Value  |  Description  | 

|Stored_P  |P_And_Q_  |  variable!  ‘  The  propagated  error  covariance  | 

I  j Matrices  j  j  matrix  which  is  stored  by  part  | 
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3.3.3.1.9.2.10.2.8  LIMITATIONS 
None. 

3.3.3.1.9.2.10.3  P  UNIT  DESIGN 

This  unit  returns  the  current  value  of  the  P  matrix 

3.3.3.1.9.2.10.3.1  REQUIREMENTS  ALLOCATION 
This  part  meets  CAMP  requirement  R146. 

3.3.3.1.9.2.10.3.2  LOCAL  ENTITIES  DESIGN 
None. 

3.3.3.1.9.2.10.3.3  INPUT/OUTPUT 
FORMAL  PARAMETERS: 

The  following  table  describes  this  unit's  formal  parameters: 


|  Name 

1  Type 

|  Mode  |  Description  | 

|<returned  value> 

1 

| P_And_Q_ 
[Matrices 

|out  | Stored  Propagated  Error  | 

|  | Covariance  matrix  j 

3.3.3.1.9.2.10.3.4  LOCAL  DATA 
None. 

3.3.3.1.9.2.10.3.5  PROCESS  CONTROL 


Not  applicable. 


3.3.3.1.9.2.10.3.6  PROCESSING 

The  following  describes  the  processing  performed  by  this  part: 
function  P  return  P_And_Q_Matrices  is 
begin 

return  Stored_P; 
end  P$ 
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3.3.3.1.9.2.10.3.7  UTILIZATION  OF  OTHER  ELEMENTS 
UTILIZATION  OF  OTHER  ELEMENTS  IN  TOP  LEVEL  COMPONENT: 

The  following  tables  describe  the  elements  used  by  this  part  but  defined 
elsewhere  in  the  parent  top  level  component: 

Data  types: 

The  following  table  summarizes  the  types  required  by  this  part  and  defined  in 
the  parent  component: 


|  Name 

|  Base  Type 

|  Description  | 

|P  and  Q  Matrices 

j  private 

|Data  type  of  n  x  n  P  and  Q  matrix | 

Data  objects: 

The  following  table  summarizes  the  objects  required  by  this  part  and  defined  in 
the  parent  component: 

|  Name  |  Type  |  Value  |  Description  | 

|Stored_P  |P_and_Q_  |  variable)  The  current  propagated  error 

|  (Matrices  j  j  covariance  matrix 


3.3.3.1.9.2.10.3.8  LIMITATIONS 
None. 

3. 3. 3. 1.9. 3  STATE jrRANSITION_MATRIX_MANAGER  PACKAGE  DESIGN  (CATALOG  #P166-0) 

This  LLCSC  is  a  generic  package  which  manages  the  State  Transition  Matrix, 
commonly  known  as  the  Phi  matrix.  It  consists  of  an  Initialization  procedure, 
a  Propagation  function,  and  a  function  which  returns  the  stored  PropagatedJPhi 
value. 

The  decomposition  for  this  part  is  the  same  as  that  shown  in  the  Top-Level 
Design  Document. 

3. 3. 3. 1.9. 3.1  REQUIREMENTS  ALLOCATION 
This  part  meets  CAMP  requirement  R148. 


3. 3. 3. 1.9. 3. 2  LOCAL  ENTITIES  DESIGN 
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3. 3. 3, 1.9. 3. 3  INPUT/OUTPUT 
GENERIC  PARAMETERS: 

Data  types: 

The  following  ’■able  summarizes  the  generic  formal  types  required  by  this  part: 


j  Name 

|  Base  Type 

|  Description 

1 

(Integrated  F 

| private 

|Data  type  for  n 

x  n  F  integration] 

(Matrices 

J 

j matrix 

1 

1 

1 

1 

| Phi_Matrices 

1 

| private 

1 

|Data  type  for  n 

1 

x  n  Phi  matrix  1 

Subprograms : 

The  following  table  summarizes  the  generic  formal  subroutines  required  by  this 
part: 


|  Name  |  Type  |  Description 


| Set  To  Identity  | function  |Sets  a  Phi  matrix  to  the  Identity  j 

I  I  .  i  ! 

I"*"  | function  (Multiplies  an  Integrated  F  matrix  by  | 

j  j  jPhi  matrix  yielding  a  Phi  matrix  j 


3. 3. 3. 1.9. 3. 4  LOCAL  DATA 
Data  objects: 

The  following  table  describes  the  data  objects  maintained  by  this  part: 

|  Name  (  Type  |  Value  j  Description  | 

|Prop_Phi  |Phi_  |  variable |  The  propagated  state  transition  | 

i  ~  (Matrices  j  j  matrix  \ 


3. 3. 3. 1.9. 3. 5  PROCESS  CONTROL 
Not  applicable. 


3. 3. 3. 1.9. 3. 6  PROCESSING 

The  following  describes  the  processing  performed  by  this  part: 
separate  (Kalman_Filter_Common_Parts) 
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package  body  State_Transition_Matrix_Manager  is 
Prop_Phi  :  Phi_Matrices; 
end  State_Transition_Matrix_Manager; 

3. 3. 3. 1.9. 3. 7  UTILIZATION  OF  OTHER  ELEMENTS 
None. 

3. 3. 3. 1.9. 3. 8  LIMITATIONS 
None. 

3. 3. 3. 1.9. 3. 9  LI.CSC  DESIGN 
None. 

3.3.3.1.9.3.10  UNIT  DESIGN 

3.3.3.1.9.3.10.1  INITIALIZE  UNIT  DESIGN 

This  unit  initializes  the  Propagated  Phi  matrix  to  the  identity. 

3.3.3.1.9.3.10.1.1  REQUIREMENTS  ALLOCATION 
This  part  meets  CAMP  requirement  R148 

3.3.3.1.9.3.10.1.2  LOCAL  ENTITIES  DESIGN 
None. 

3.3.3.1.9.3.10.1.3  INPUT/OUTPUT 
None. 

3.3.3.1.9.3.10.1.4  LOCAL  DATA 
None. 

3.3.3.1.9.3.10.1.5  PROCESS  CONTROL 
Not  applicable. 
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3.3.3.1.9.3.10.1.6  PROCESSING 

The  following  describes  the  processing  performed  by  this  part: 
procedure  Initialize  is 
begin 

Set_To_Identity_Matrix  (Matrix  =>  Prop_Phi); 
end  Initialize; 


3.3.3.19.3.10.1.7  UTILIZATION  OF  OTHER  ELEMENTS 
UTILIZATION  OF  OTHER  ELEMENTS  IN  TOP  LEVEL  COMPONENT: 


The  following  tables  describe  the  elements  used  by  this  part  but  defined 
elsewhere  in  the  parent  top  level  component: 

Data  types: 

The  following  table  summarizes  the  data  types  defined  by  the  parent  component: 


]  Name 

|  Base  Type 

|  Description  | 

| Phi_Ma trices 

| private 

|Data  type  for  n  x  n  Phi  matrix  | 

Data  objects: 

The  following  table  summarizes  the  objects  required  by  this  part  and  defined  in 
the  parent  component: 


|  Name 
| Prop_Phi 


|  Type  |  Value  j 


Description 


I 


|Integrated_|  variable |  The  propagated  state  transition 
|F_Matrix  j  j  matrix 


3.3.3.1.9.3.10.1.8  LIMITATIONS 
None. 


OOOI  n  n  1 A  O  TmAnt/itmn  imrm  nwnr/iM 

O.O.O. x. 7,0.1  Uuli  DaSlVjW 


This  unit  propagates  a  new  Propagated  Phi  matrix. 
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3.3.3.1.9.3.10.2.1  REQUIREMENTS  ALLOCATION 
This  part  meets  CAMP  requirement  R148. 

3.3.3.1.9.3.10.2.2  LOCAL  ENTITIES  DESIGN 
None. 

3.3.3.1.9.3.10.2.3  INPUT/OUTPUT 
FORMAL  PARAMETERS: 

The  following  table  describes  this  unit's  formal  parameters: 


i  Name 

1  Type 

|  Mode  |  Description  | 

|  Phi 

i 

|Integrated_ 
|F  Matrices 

1  in 

1 

(Current  value  for  the  j 

(State  Transition  Matrix  j 

3.3.3.1.9.3.10.2.4 

LOCAL  DATA 

None. 

3.3.3.1.9.3.10.2.5 

PROCESS  CONTROL 

Not  applicable. 

3.3.3.1.9.3.10.2.6 

PROCESSING 

The  following  describes  the  processing  performed  by  this  part: 

procedure  Propagate  (Integrated_F  :  in  Integrated_F_Matrices)  is 
begin 

Prop_Phi  :=  Integrated_F  *  Prop_Phi; 
end  Propagate; 


3.3.3.1.9.3.10.2.7  UTILIZATION  OF  OTHER  ELEMENTS 

UTILIZATION  OF  OTHER  ELEMENTS  IN  TOP  LEVEL  COMPONENT: 

The  following  tables  describe  the  elements  used  by  this  part  but  defined 
elsewhere  in  the  parent  top  level  component: 

Data  types: 


k">,\  .  VVJV  'h  .%  ,•> 


& 
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The  following  table 
the  parent  component 

summarizes  the 

types  required  by  this  part 

and 

defined 

|  Name 

|  Base  Type 

j  Description 

1 

|Integrated_F_ 
j  Matrices 

1 

| private 

1 

(Data  type  for  N  by  N  matrix 
|F  Integration 

i 

for 

1 

1 

| 

1 

| Phi  Matrices 

1  " 

| private 

1 

|Data  Type  for  N  by  N  Phi  matrix 

1 

1 

1 

1 

Data  objects: 

The  following  table  summarizes  the  objects  required  by  this  part  and  defined  in 
the  parent  component: 

|  Name  |  Type  |  Value  |  Description  | 

|Prop_Phi  |Integrated_|  variable)  The  propagated  state  transition  | 

|  |F_Matrices  j  |  matrix  j 


3.3,3.1.9.3.10.2.8  LIMITATIONS 
None. 

3.3.3.1.9.3.10.3  PROPAGATED_PHI  UNIT  DESIGN 

This  unit  returns  the  current  value  of  the  Propagated  Phi  matrix 

3.3.3.1.9.3.10.3.1  REQUIREMENTS  ALLOCATION 
This  part  meets  CAMP  requirement  R148. 

3.3.3.1.9.3.10.3.2  LOCAL  ENTITIES  DESIGN 
None. 

3.3.3.1.9.3.10.3.3  INPUT/OUTPUT 
FORMAL  PARAMETERS: 

The  following  table  describes  this  unit's  formal  parameters: 
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|  Name 

|  Type  |  Mode 

|  Description  | 

| <returned  value> 

1 

1  Phi  Matrices | out 

{Stored  Propagated  State  { 

1  1 

[Transition  matrix  { 

3.3.3.1.9.3.10.3.4 

None. 

LOCAL  DATA 

3.3.3.1.9.3.10.3.5 

Not  applicable. 

PROCESS  CONTROL 

3.3.3.1.9.3.10.3.6 

PROCESSING 

The  following  describes  the  processing  performed  by  this  part: 
function  Propagated_Phi  return  Phi_Matrices  is 
begin 

return  Prop_Phi; 
end  Propagated_Phi 5 

3.3.3.1.9.3.10.3.7  UTILIZATION  OF  OTHER  ELEMENTS 

UTILIZATION  OF  OTHER  ELEMENTS  IN  TOP  LEVEL  COMPONENT: 

The  following  tables  describe  the  elements  used  by  this  part  but  defined 
elsewhere  in  the  parent  top  level  component: 

Data  types: 

The  following  table  summarizes  the  types  required  by  this  part  and  defined  in 
the  parent  component: 


|  Name 

|  Base  Type 

|  Description  | 

|Phi_Matrices 

| private 

|Data  type  for  n  x  n  Phi  matrix  j 

Data  objects: 

The  following  table  summarizes  the  objects  required  by  this  part  and  defined  in 
the  parent  component: 
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|  Name 

1  Type 

|  Value  |  Description  | 

|Prop  Phi 

1  “ 

|Phi_ 
j Matrices 

|  variable |  The  propagated  state  transition  | 
j  j  matrix  j 

3.3.3.1.9.3.10.3.8  LIMITATIONS 


None. 


3.3.3.1.10  UNIT  DESIGN 


None. 


] 


j 

i 

1 

I 

a 

1 

i 


««■»»» 
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package  body  Kalman_Filter_Common_Parts  is 
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package  body  State_Transition_And_Process_Noise_Matrices_Manager  is  separate; 


package  body  Error_Covariance_Matrix_Manager 
package  body  State_Transition_Matrix_Manager 
end  Kalman  Filter  Common  Parts; 


is  separate; 
is  separate; 
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separate  (Kalman_Filter_Common_Parts) 

package  body  State_Transition_And_Process_Noise_Matrices_Manager 

Prop_Phi  :  Phi_Matrices; 

Prop_Q  :  Q_Matrices; 

pragma  PAGE; 

procedure  Initialize  is 

begin 

Set_To_Identity_Matrix  (Source  =>  Prop_Phi); 
Set_To_Zero_Matrix  (Source  =>  Prop_Q); 
end  Initialize; 
pragma  PAGE; 

procedure  Propagate  (Integrated_F  :  in  Integrated_F_Matrices; 

Q  :  in  Integrated_Q_Matrices; 

Dt  :  in  Time_Intervals)  is 

Phi  ;  Integrated_F_Matrices; 

begin 

—  Propagate  Phi 

Phi  :=  Add_To_Identity  (Source_Matrix  =*>  IntegratedF) ; 
Prop_Phi  :=  Phi  *  Prop_Phi; 

—  Propagate  Q 

Prop_Q  :=  Q  *  Dt  +  Aba_Transpose(  Phi,  Prop_Q  )  ; 
end  Propagate; 
pragma  PAGE; 

procedure  Get_Current  (Propagated_Phi  :  out  Phi_Matrices; 

Propagated_Q  :  out  Q_Matrices  )  is 

begin 

Propagated_Phi  :=  Prop_Phi; 

Propagated_Q  :=  Prop_Q; 
end  Get_Current; 
pragma  PAGE; 

function  Propagated_Phi  return  Phi_Matrices  is 


begin 
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return  Prop_Phi; 
end  Propagated_Phi; 

end  State_Transition_And_Process_Noise__Matrices_Manager; 
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separate  (Kalman_Filter_Common_Parts) 
package  body  Error_Covariance_Matrix_Manager  is 
Stored_P  :  P_And_Q_Matrices; 
pragma  PAGE? 

procedure  Initialize  (Initial_P  :  in  P_And_Q_Matrices)  is 
begin 

Stored_P  :=  Initial_P; 
end  Initialize; 
pragma  PAGE; 

procedure  Propagate  (Propagated_Phi  :  in  Phi_Hatrices; 

Propagated_Q  :  in  P_And_Q_Matrices)  is 

begin 

Stored_P  :=  Aba_Transpose(  Propagated_Phi ,  Stored_P  )  +  Propagated_Q 
end  Propagate; 
pragma  PAGE; 

function  P  return  P_And_Q_Matrices  is 
begin 

return  StoredP; 
end  P; 

end  Error_Covariance_Matrix_Manager ; 
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separa  te  ( Kalman_F i 1 1  er_Common_Par  t  s ) 

package  body  State_Transition_Matrix_Manager  is 

Prop_Phi  :  Phi_Matrices; 

pragma  PAGE; 

procedure  Initialize  is 

begin 

Set_To_Identity_Matrix  (Matrix  =>  Prop_Phi); 
end  Initialize; 
pragma  PAGE; 

procedure  Propagate  (Integrated_F  :  in  Integrated_F_Matrices)  is 
begin 

Prop_Phi  :=  Integrated_F  *  Prop_Phi; 
end  Propagate; 
pragma  PAGE; 

function  PropagatedPhi  return  Phi_Matrices  is 
begin 

return  Prop_Phi; 
end  Propagated_Phi ; 
end  StateTransitionMatrix  Manager; 
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3. 3. 3. 2  KALMAN  FILTER  COMPACT  H  PARTS  (BODY)  TLCSC  (CATALOG  #P137-0) 

This  part,  which  is  designed  as  an  Ada  package,  contains  bodies  for  all  CAMP 
parts  which  can  be  used  to  implement  a  Kalman  Filter  when  a  compact  Measurement 
Sensitivity  Matrix  (Compact  H  Matrix)  is  used 

The  decomposition  for  this  part  is  the  same  as  that  shown  in  the  Top-Level 
Design  Document. 


3. 3. 3. 2.1  REQUIREMENTS  ALLOCATION 

The  following  chart  summarizes  the  allocation  of  CAMP  requirements  to  this 
Tlcscs 


|  Name 

1 

Requirements  Allocation 

|  Kalman  Update 

1 

R147 

j Compute  Kalman  Gain 

R149 

j Update  Error  Covariance  Matrix 

1 

R150 

| Update  State  Vector 

1 

R151 

|Sequentially_Update_Covariance_Matrix_and_ 

|  State  Vector 

1 

1 

R152 

3. 3. 3. 2. 2  LOCAL  ENTITIES  DESIGN 


None. 

3. 3. 3. 2. 3  INPUT/GUTPUT 
None. 

3. 3. 3. 2. 4  LOCAL  DATA 
None. 

3. 3. 3. 2. 5  PROCESS  CONTROL 
Not  applicable. 

3. 3. 3. 2. 6  PROCESSING 

The  following  describes  the  processing  performed  by  this  part: 

package  body  Kalman_Filter_Compact_H_Parts  is 

function  Compute_Kalman_Gain 

(P  :  P_Matrices; 

Measurement  Number  :  Measurement  Indices; 
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Compact_H  :  Compact_H_Matrices; 

Measurement_Variance  :  Measurement_Variance_Vectors) 
return  K  Column  Vectors  is  separate; 


procedure 


procedure 


Update  Error  Covariance  Matrix 


(P  :  in 
Measurement_Number  :  in 
K  :  in 
Compact_H  :  in 
is  separate; 

Update_State_Vector 

(X  :  in 
Z  :  in 
K  :  in 
Measurement_Number  :  in 
Compact_H  :  in 
is  separate; 


out  P_Matrices; 

Measurement_Indices; 
K_Column_Vectors; 
Compact  H  Matrices) 


out  State_Vectors; 

Measurement_Vectors ; 
K_Column_Vectors; 
Measurement_Indices ; 
Compact_H_Matrices) 


package  body  Sequentially_Update_Covariance_Matrix_and_State_Vector 
is  separate; 


package  body  Kalman_Update  is  separate; 


procedure  UI/date_Error_Covariance_Matrix_General_Form 


(P  :  in  out 

Measurement_Number  :  in 

K  :  in 

Compact_H  :  in 

Measurement_Variance  :  in 

is  separate; 


P_Matrices; 

Measur emen  t_Ind ices; 
K_Column_Vectors ; 

Compac  t_H_Ma trices; 
Measurement  Variance  Vectors) 


end  Kalman_F i 1 1  er_Compac  t_H_Par  t s ; 

3. 3. 3. 2. 7  UTILIZATION  OF  OTHER  ELEMENTS 


None. 


3. 3. 3. 2. 8  LIMITATIONS 
None . 


3. 3. 3. 2. 9  LLCSC  DESIGN 

3. 3. 3. 2.9.1  SEQUENTIALLY  UPDATE  COVARIANCE  MATRIX  AND  STATE  VECTOR  PACKAGE  DESIGN 
(CATALOG  #P141-0) 

This  LLCSC  is  a  generic  package  wh'ch  contains  one  procedure,  "Update",  which 
updates  the  Covariance  Matrix,  P,  and  state  Vector,  X. 

The  decomposition  for  this  part  is  the  same  as  that  shown  in  the  Top-Level 
Design  Document. 
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3. 3. 3. 2. 9. 1.1  REQUIREMENTS  ALLOCATION 
This  part  meets  CAMP  requirement  R152. 


3. 3. 3. 2. 9. 1.2  LOCAL  ENTITIES  DESIGN 
Packages : 

As  described  above,  this  package  body  instantiates  Part  R149,  Compute  Kalman 
Gain,  part  R150,  Update  Error  Covariance  Matrix,  and  part  R151,  Update  State 
Vector 


3. 3. 3. 2. 9. 1.3  INPUT/OUTPUT 
GENERIC  PARAMETERS: 

Data  types: 

The  following  table  summarizes  the  generic  formal  data  types  required  by  this 
part: 


|  Name 

1  . .  . . 

Base  Type 

Description 

1  -  - 

|State_Indices 

discrete 

Index  to  the  arrays  which  depend 
on  the  number  of  states 

j  Measurement_Indices 

| 

discrete 

Index  to  the  arrays  which  depend 
on  the  number  of  measurements 

1 

|Kalman_Filter_ 
j  Elements 

floating 
point  type 

Elements  making  up  the 

Kalman  Filter  aggregates 

1 

| P_Mat rices 

private 

Data  type  of  P  matrix 

1 

|Measurement_ 
j  Variance_Vectors 

vector 

Vector  indexed  by  Measurement 
Indices  containing  Kalman_Filter_ 
Elements 

1 

|  Measurement_Vectors 

1 

1 

vector 

Vector  indexed  by  Measurement 
Indices  containing  Kalman_Firter_ 
Elements  ~ 

1 

| P_Row_Vectors 

s 

vector 

Vector  indexed  by  Stata_Indices 
containing  Kalman_Filter_Elements 

1 

| K_Column_Vec  tors 

vector 

Vector  indexed  by  StateJEndices 
containing  Kalman_Filter_Elements 

1 

|State_Vectors 

1 

vector 

Vector  indexed  by  State_Indices 
containing  Kalman_filter_Elements 

1 

| Compact_H_Matrices 

vector 

Data  type  of  Compact  H  matrix 
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Subprograms: 

The  following  table  summarizes  the  generic  formal  subroutines  required  by  this 
part: 


Name 

Type 

|  Description 

Element 

function 

(Extracts  an  element  of  a  P  Matrix 
| 

Row 

function 

1 

| Extracts  a  row  of  a  P  matrix 

n*n 

function 

I 

|A  K  Column  Vector  is  multiplied 
j  by  the  transpose  of  a  P  Row  vector, 
(yielding  a  P  matrix 

1 

n_n 

function 

1 

|Two  P  matrices  are  added,  yielding  a 
j Symmetric  matrix 

1 

ti.tt 

function 

1 

| Add  a  state  vector  and  a  K  column 
|vector,  yielding  a  state  vector 

l 

it  *n 

function 

(Multiply  a  K  column  vector  by  a  Kal- 
jman  Filter  Element,  yielding  a  K 
j column  vector 

3. 3. 3. 2. 9. 1.4  LOCAL  DATA 


None. 


3. 3. 3. 2. 9. 1.5  PROCESS  CONTROL 


Not  applicable. 


3. 3. 3. 2. 9. 1.6  PROCESSING 

The  following  describes  the  processing  performed  by  this  part: 
separate  (Kalman_Filter  Compact_H_Parts) 

package  body  SequentialIy_Update_Covariance_Matrix_and_State_Vector  is 
K  :  K  Column  Vectors; 


function  ComputeJC  is  new  Compute_Kalman_Gain 

(Stale_lndices  =>  State_Indices, 

Measurement_Indices  =>  Measurement_Indices, 

Kalman  Filter_Elements  =>  Kalman  Filter_Elements, 

P_Matrices  =>  P_MatrIces, 

Measurement_Vari ance_Vectors 

=>>  Measurement_Variance_Vectors, 
K_Column_Vectors  =>  K_Column_Vectors, 

Compact_H_Ma trices  =>  Compact_H_Matrices) ; 


1 


% 
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procedure  Update  P  is  new  Update  Error  Covariance  Matrix 


(State_Indices 
Measurement_Indices 
Kalman_Filter_Elements 
P_Matrices 

P_R°w_Vect°rs 

K_Column_Vectors 
Compact  H  Matrices 


=>  State_Indices, 

=>  Measurement_Indices, 

=>  Kalman_Filter_Elements, 
=>  P_Matrices, 

=>  P_Row_Vectors, 

=>  K_Column_Vectors, 

=>  Compact  H  Matrices); 


procedure  Update_X  is  new  Update_State_Vector 

(State_Indices  =>  State_Indices, 

Measurement_Indices  =>  Measurement_Indices, 

Kalman_Filter_Elements  =>  Kalman_Filter_Elements, 

Measurement_Vectors  =>  Measurement_Vectors, 

K_Column_Vectors  =>  K_Column_Vectors, 

State_Vectors  =>  State_Vectors, 

Compact  H  Matrices  =>  Compact  H  Matrices); 


end  Sequentially_Update_Covariance_Matrix_and_State_Vector; 


3. 3. 3. 2. 9. 1.7  UTILIZATION  OF  OTHER  ELEMENTS 
UTILIZATION  OF  OTHER  ELEMENTS  IN  TOP  LEVEL  COMPONENT: 


The  following  tables  describe  the  elements  used  by  this  part  but  defined 
elsewhere  in  the  parent  top  level  component: 


Subprograms  and  task  entries: 


The  following  table  summarizes  the  subroutines  and  task  entries  required  by 
this  part  and  defined  elsewhere  in  the  parent  top  level  component: 


Name 

|  Type 

Source 

|  Description 

_l  _ _ 

1 

Compute  Kalman 

1  generic 

Kalman 

1  . 

(Computes  the  Kalman  Gain  Vector, 

Gain 

j  function 

Filter 

|K 

I 

I 

Common_ 

1 

Update  State 

| generic 

Kalman 

(Updates  the  State  Vector,  X 

1 

1 

Vector 

j  procedure 

Filter 

1 

1 

1 

Common 

1 

Update  Error 

1 

j generic 

Kalman 

1 

| Updates  the  Error  Covariance 

1 

1 

|  Covariance  Matrix  1  procedure 

Filter 

(Matrix,  P 

1 

1 

Common 

1 

1 

3. 3. 3. 2. 9. 1.8  LIMITATIONS 
None. 


CAMP  Software  Detailed  Design  Document 


Page  350 


3. 3. 3. 2. 9. 1.9  LLCSC  DESIGN 
None. 

3.3.3.2.9.1.10  UNIT  DESIGN 

3.3.3.2.9.1.10.1  UPDATE  UNIT  DESIGN 

This  unit  is  a  procedure  which  does  the  update  of  the  Covariance  Matrix,  P,  and 
the  State_Vector,  X. 

3.3.3.2.9.1.10.1.1  REQUIREMENTS  ALLOCATION 
This  parts  meets  CAMP  Requirement  R152. 

3.3.3.2.9.1.10.1.2  LOCAL  ENTITIES  DESIGN 
None. 

3.3.3.2.9.1.10.1.3  INPUT/OUTPUT 
FORMAL  PARAMETERS: 

The  following  table  describes  this  part's  formal  parameters: 


|  Name 

.... 

1  Type 

|  Mode 

I  Description 

1 

jP 

| P_Ma trices 

in/out 

| Error  Covariance  Matrix  (P) 
j  to  be  updated 

i 

jx 

| State_Vectors 

in/out 

| State  Vector  (X)  to  be  updatd 

jz 

|  Measurement 
j  Vectors 

in 

| Current  Measurement  Vcctor(Z) 

1 

I 

|Compact_H 

1 

1 

|Compact_H_ 
j Matrices 

i 

in 

i 

1 

| Current  Measurement  Sensitiv- 
jity  Array  (Compact  H) 

1 

|  Measurement 
j  Variance 

i  i 

|  Measurement  |in 

j Variance_Vectors  j 

1 

| Current  Measurement  Variance 
j  Array 
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3.3.3.2.9.1.10.1.5  PROCESS  CONTROL 


Not  applicable. 


3.3.3.2.9.1.10.1.6  PROCESSING 


The  following  describes  the  processing  performed  by  this  part: 


procedure  Update 


(P 

X 

Z 

Compact  H 


Measurement  Variance  :  in 


in  out  P_Ma trices; 
in  out  State_Vectors; 
in  Measurement_Vectors; 
in  Compact_H_Matrices; 


Measurement  Variance  Vectors)  is 


begin 


for  Measurement  Number  in  Measurement  Indices  loop 


K  :=  Compute  K  (P 


=>  P, 


Measurement_Number  =>  Measurement_Number, 
Compact_H  ~  =*>  Compact_H, 

Measurement_Variance  =>  Measurement_Variance) ; 

Update_P  (P  *>  P, 

~  Measurement_Number  =>  Measurement_Number, 

K  =«>  K, 

•Compact_H  =>  Compact_H); 

Update_X  (X  *>  X, 

Z  =>  Z, 

K  =.>  K, 

Measurement_Number  =>  Measurement_Number, 

Compact  H  =>  Compact  H);~ 


end  loop; 
end  Update; 


3.3.3.2.9.1.10.1.7  UTILIZATION  OF  OTHER  ELEMENTS 

UTILIZATION  OF  OTHER  ELEMENTS  IN  TOP  LEVEL  COMPONENT: 

The  following  tables  describe  the  elements  used  by  this  part  but  defined 
elsewhere  in  the  parent  top  level  component: 

Subprograms  and  task  entries: 

The  following  table  summarizes  the  subroutines  and  task  entries  required  by 
this  part  and  defined  elsewhere  in  the  parent  top  level  component: 


■  1 
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|  Name 

|  Type 

|  Source 

| Compute  Kalman 

| generic 

j  parent 

|Gain 

j  function 

| Update  State 

1 

| generic 

1 

(parent 

j Vector 

j procedure 

1 

1 

1 

|  Update  Error 

(generic 

1 

|  parent 

|  Covariance_Matrix | procedure 

Description 

Computes  the  Kalman  Gain  Vector, 
K 

Updates  the  State  Vector,  X 

Updates  the  Error  Covariance 
Matrix,  P 


Data  types: 

The  following  table  summarizes  the  types  required  by  this  part  and  defined 
elsewhere  in  the  parent  top  level  component: 


|  Name 

1 

]  Base  Type 

j  Description 

1 

| Measurement_Indices 

l 

j discrete 

j Index  to  the  arrays  which  depend 
j  on  the  number  of  measurements 
| 

i 

|P  Matrices 

1 

j  private 

1 

1 

|Data  type  of  P  matrix 

i 

| Measurement 
j  Variance_Vectors 

1 

1 

(vector 

i 

1 

| Vector  indexed  by  Measurement 
| Indices  containing  Kalman_Filter_ 
j  Elements  “ 

| 

1 

| Measurement_Vectors 

| vector 

1 

l 

1 

(Vector  indexed  by  Measurement_ 
(Indices  containing  Kalman_Filter 

(Elements 

1 

|State_Vectors 

1 

1 

( vector 

1 

1 

1 

| Vector  indexed  by  State_Indices 
j  containing  Kalman_Filter_Elements 

1 

|Compact_H  Matrices 

1 

| vector 

i 

(Data  type  of  Compact  H  matrix 

Data  objects: 

The  following  table  summarizes  the  objects  required  by  this  part  and  defined 
elsewhere  in  the  parent  top  level  component: 


|  Name 

1 

1  Type 

1  ...... 

|  Value 

1  ... 

|  Description  | 

i  . -  . _____ . .  ._  -  I 

1 

|K 

1 

IK  Column 

1 

(variable 

1  1 
[Stores  the  value  of  the  Kalman  | 

1 

1  Vectors 

1 

|Gain  Vector  (K)  j 
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3.3.3.2.9.1.10.1.8  LIMITATIONS 


3. 3. 3. 2. 9. 2  KALMAN  UPDATE  (BODY)  PACKAGE  DESIGN  (CATALOG  #P142-0) 


This  LLCSC  is  a  generic  package  body  which  contains  1  procedure,  "Update"  which 
updates  the  State  Vector,  X,  given  the  old  X  vector,  the  Z  vector,  the  K 
vector,  the  Measurement  Number,  and  the  Compact  H  array. 


The  decomposition  for  this  part  is  the  same  as  that  shown  in  the  Top-Level 
Design  Document. 


3. 3. 3. 2. 9. 2.1  REQUIREMENTS  ALLOCATION 


This  part  meets  CAMP  requirement  R147. 


3. 3. 3. 2. 9. 2. 2  LOCAL  ENTITIES  DESIGN 


None. 


3. 3. 3. 2. 9. 2. 3  INPUT/OUTPUT 


GENERIC  PARAMETERS: 


Data  types: 


The  following  table  summarizes  the  generic  formal  data  types  required  by  this 
part: 
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1  Name 

1  _  _ 

Base  Type 

|  Description  j 

! 

| State_Indices 

l 

discrete 

j Index  to  the  arrays  which  depend  j 
j  on  the  number  of  states  j 

(Measurement  Indices 

discrete 

i  ! 

| Index  to  the  arrays  which  depend  | 

|  on  the  number  of  measurements  j 

i  i 

1 

1  Kalman  Filter 

floating 

(Elements  making  up  the  | 

j  Elements 

1 

point  type 

(Kalman  Filter  aggregates  j 

l  i 

1 

|Phi_Matrices 

private 

1 

|Data  type  of  Phi  matrix  j 

! 

| P_And_Q_Matrices 

private 

i  1 

|Data  type  of  a  P  and  Q  matrix  j 

i  i 

(Measurement 
j  Variance  Vectors 

1 

vector 

1 

| Vector  indexed  by  Measurement  j 

j Indices  containing  Kalman  Filter  j 

(Elements  ~[ 

|  | 

1 

| Measurement_Vectors 

i 

i 

vector 

i 

| Vector  indexed  by  Measurement  | 

(Indices  containing  Kalman_Filter  j 

(Elements  j 

I  1 

1 P_Row_Vectors 

1 

vector 

1  ! 

| Vector  indexed  by  State  Indices  ( 
j  containing  Kalman_Filter_Elements  j 

| K_Column_Vectors 

vector 

!  1 
| Vector  indexed  by  State_Indices  j 
j  containing  Kalman_Filter_Elements  j 

| State_Vectors 

vector 

i  I 

| Vector  indexed  by  State_Indices  | 
j  containing  Kalman_Filter_Elements  j 

| Compact_H_Matrices 

vector 

1  1 
|Data  type  of  Compact  H  matrix  j 

Subprograms: 

The  following  table  summarizes  the  generic  formal  subroutines  required  by  this 
part: 
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|  Name 

1  Type 

(  Description 

1 

l 

il 

* 

j Element 

1 

1 

j  function 

1 

I 

(Extracts  an  element  of  a  P  and  Q 

(Matrix 

1 

1 

1 

i 

\ 

|Row 

I 

I 

j  function 

1 

(Extracts  a  row  of  a  P  and  Q  matrix 

1 

1 

I 

j 

1 

I  Phi  P  Phi 

| function 

(Performs  an  ABA  transpose  on  a  Phi 

1 

ij 

|  Transpose 

l 

1 

l 

j Matrix  and  a  P  and  Q  Matrix 

1 

1 

l 

£ 

j  »*" 

1 

(function 

|A  K  Column  Vector  is  multiplied 

1 

1 

1 

1 

j  by  the  transpose  of  a  P  Row  vector, 

1 

1 

(yielding  a  P  and  Q  matrix 

1 

1 

1 

1 

| function 

1 

1 

i 

|Two  P  and  Q  matrices  are  added, 
j yielding  a  P  and  Q  matrix 

i 

1 

1 

| 

l 

| "+" 

1 

i 

| function 

I 

1 

1 

(Add  a  state  vector  and  a  K  column 
(vector,  yielding  a  state  vector 

i 

1 

1 

1 

1 

l 

i 

ii*n 

| function 

1 

(Multiply  a  K  column  vector  by  a  Kal- 

1 

1 

1 

jman  Filter  Element,  yielding  a  K 

1 

1 

1 

j column  vector 

1 

i 

-i 

3. 3. 3. 2. 9. 2. 4  LOCAL  DATA 
None. 


3. 3. 3. 2. 9. 2. 5  PROCESS  CONTROL  f 

Not  applicable.  | 

i 

1 

3. 3. 3. 2. 9. 2. 6  PROCESSING  • 

The  following  describes  the  processing  performed  by  this  part:  j 

with  Kalman_Filter  Common_Parts; 

separate  (Kalman_Fllter_Compact_H_Parts)  • 

package  body  Kalman_Update  is 

| 

package  P_Manager  is  new  ; 

Kalman_Filter_Comraon_Parts. Error  Covariance_Matrix_Manager  ; 

(Phi_Matrices  =>  Phi_MatrIces,  ‘ 

P_And_Q_Ma trices  =>  P_And_Q_Matrices) ; 

package  Update_P_And  X  is  new  i 

Sequentially  Update_Covariance_Matrix_And_State_Vector  .  3 

(State_Incfices  =>  Stati_Indices,~  \ 

Measurement_Indices  =>  Measurement_Indices, 

Kalman_Filter_Elements  =>  Kalman_Filter_Elements, 

P_Matrices  =>  P_And_Q_Matrices,  < 

Measurement  Variance  Vectors 
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Measurement_Vectors 

P_R°w_Vect°rs 

K_Column_Vec  tors 

State_Vectors 

Compact_H_Matrices 

Zero  State  Vector  :  State  Vectors 


=>  Measurement_Variance_Vectors 
=>  Measurement_Vectors, 

=>  P_Row_Vectors, 

=>  K_Column_Vectors, 

=>  State_Vectors, 

=>  Compact_H_Matrices) ; 

(others  =>  0.0); 


end  Kalman_Update; 


I 


wi 

‘i 


3. 3. 3. 2. 9. 2. 7  UTILIZATION  OF  OTHER  ELEMENTS 

The  following  library  units  are  with'd  by  this  part: 

1.  Kalman_Filter_Common_Parts 

UTILIZATION  OF  EXTERNAL  ELEMENTS: 

Packages : 

The  following  table  summarizes  the  external  packages  required  by  this  part: 


Name  i  Type  |  Source  |  Description 


Covariance_Matrixj generic  |Kalman_  j Manages  the  Covariance  Matrix  (P) 
_Manager  j package  |Filter_  | 

!  j  Common"  j 


UTILIZATION  OF  OTHER  ELEMENTS  IN  TOP  LEVEL  COMPONENT: 

The  following  tables  describe  the  elements  used  by  this  part  but  defined 
elsewhere  in  the  parent  top  level  component: 


Packages : 


The  following  table  summarizes  the  packages  required  by  this  part  and  defined  | 

elsewhere  in  the  parent  top  level  component:  j 


j  Name  |  Type 

|  | 

|  Source 

1 

Description  j 

l  1 

| Sequentially  | generic 

j  Update  Covariance  j  package 

| parent 

1 

1  Updates 

i 

the  P  matrix  and  X  Vector | 

j Matrix  And  j 

|State_Vector  | 

1 

1 

1 

3. 3. 3. 2. 9. 2. 8  LIMITATIONS 
None. 
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3. 3. 3. 2. 9. 2. 9  LLCSC  DESIGN 
None. 

3.3.3.2.9.2.10  UNIT  DESIGN 

3.3.3.2.9.2.10.1  UPDATE  UNIT  DESIGN 

This  unit  is  a  procedure  which  does  the  Kalman  Update 

3.3.3.2.9.2.10.1.1  REQUIREMENTS  ALLOCATION 
This  parts  meets  CAMP  Requirement  R147. 

3.3.3.2.9.2.10.1.2  LOCAL  ENTITIES  DESIGN 
None. 


3.3.3.2.9.2.10.1.3  INPUT/OUTPUT 
FORMAL  PARAMETERS: 

The  following  table  describes  this  part's  formal  parameters: 


Name 

Type 

|  Mode 

Description 

X 

StateVectors 

|  in/out 

State  Vector  (X)  to  be  updatd 

P 

P_And_Q_Ma t r i ces 

| in/out 

Error  Covariance  Matrix  (P) 
to  be  updated 

Z 

Measurement 

Vectors 

jin 

Current  Measurement  Vector(Z) 

Compact_H 

Compact_H_ 

Matrices 

lin 

1 

Current  Measurement  Sensitiv¬ 
ity  Array  (Compact  H) 

Measurement_ 

Variance 

Measurement 

Variance_Vectors 

i 

1  in 

1 

Current  Measurement  Variance 
Array 

Propagated_Phi 

Phi_Matrices 

1 

|  in 

1 

1 

Current  value  of  the  propa¬ 
gated  State  Transition  Matrix 

Propagated_Q 

P_And_Q_Ma  trices 

1  in 

1 

Current  value  of  the  propa¬ 
gated  Process  Noise  Matrix 

CAMP  Software  Detailed  Design  Document 


Page  358 


3.3.3.2.9.2.10.1.4  LOCAL  DATA 
None. 


3.3.3.2.9.2.10.1.5  PROCESS  CONTROL 
Not  applicable. 


3.3.3.2.9.2.10.1.6  PROCESSING 

The  following  describes  the  processing  performed  by  this  part: 
procedure  Update 


(X 
P 

Z  :  in 
Compact_H  :  in 
Measurement_Variance  :  in 
Propagated_Phi  :  in 
Propagated~Q  :  in 


begin 

—  Propagate  the  Error  Covariance  Matrix 
P_Manager. Initialize  (Initial_P  *>  P); 


:  in  out  State_Vectors; 

:  in  out  P_And_Q_Matrices; 

Measurement_Vectors; 
Compact_H_Ma trices ; 
Measurement_Variance_Vectors ; 
Phi_Matrices; 

P  And  Q  Matrices)  is 


P_Manager. Propagate  CPropagated_Phi  =>  PropagatedPhi , 

Propagated~Q  =>  PropagatedQ); 


P  :=  P  Manager. P; 


—  If  the  state  vector  is  not  zero,  multiply  it  by  Propagated  Phi 
if  X  /=  Zero_State_Vector  then 
X  ;=  Propagated_Phi  *  X; 
end  if; 


—  Update  Error  Covariance  Matrix  and  State  Vector 


Update_P_And_X. Update  (P  =>  P, 

X  =>  X, 

Z  =>  Z, 

Compact_H  =>  Compact_H, 

Measurement  Variance  =>  Measurement  Variance); 


end  Update; 


m 


fry'*#' 


jmm* 
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3.3.3.2.9.2.10.1.7  UTILIZATION  OF  OTHER  ELEMENTS 

UTILIZATION  OF  OTHER  ELEMENTS  IN  TOP  LEVEL  COMPONENT: 

The  following  tables  describe  the  elements  used  by  this  part  but  defined 
elsewhere  in  the  parent  top  level  component: 

Packages : 

The  following  table  summarizes  the  subroutines  and  task  entries  required  by 
this  part  and  defined  elsewhere  in  the  parent  top  level  component: 


|  Name 

1  Type 

...  |  _ 

j  Source 

I 

|  Description  | 

|P  Manager 

1 

l 

1 

| package 

1 

1 

l 

1 

[parent 

i 

i 

i 

1  —  1 
(Manages  the  Error  Covariance  j 

jmatrix  (it  is  an  instantiation  of  j 
jpart  F.146)  j 

1  1 

| Update  P  And  X 

1 

1 

| package 

i 

i 

1 

| parent 

i 

i 

I  1 

| Updates  the  Covariance  Matrix  and| 
jx  Vector  (it  is  an  instantiation  | 
jof  part  R152)  j 

Data  types: 

The  following  table  summarizes  the  types  required  by  this  part  and  defined 
elsewhere  in  the  parent  top  level  component: 


% 

*. 

» 

4 


u 


■i 


w 


-  *■**%*’'  +  *•  '**•  • «. *vyv  * \ *» ,  * 
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|  Name 

1 

Base  Type 

Description 

1 

| State_Indices 

1 

discrete 

Index  to  the  arrays  which  depend 
on  the  number  of  states 

I 

[Measurement  Indices 

i 

discrete 

Index  to  the  arrays  which  depend 
on  the  number  of  measurements 

1  Kalman  Filter 

floating 

Elements  making  up  the 

j  Elements 

1 

point  type 

Kalman  Filter  aggregates 

1 

| Phi  Matrices 

private 

Data  type  of  Phi  matrix 

jP  And  Q  Matrices 

1  — 

private 

Data  type  of  a  P  and  Q  matrix 

i 

(Measurement 
j  Variance  Vectors 

vector 

Vector  indexed  by  Measurement 
Indices  containing  Kalman  Filter 
Elements 

(Measurement  Vectors 

i 

vector 

Vector  indexed  by  Measurement 
Indices  containing  Kalman  Filler 
Elements 

| State  Vectors 

i 

vector 

Vector  indexed  by  State  Indices 
containing  Kalman  Filter  Elements 

1 

| Compact_H_Matrices 

vector 

Data  type  of  Compact  H  matrix 

Data  objects: 

The  following  table  summarizes  the  objects  required  by  this  part  and  defined 
elsewhere  in  the  parent  top  level  component: 
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3.3.3.2.10.1.1  REQUIREMENTS  ALLOCATION 
This  part  meets  CAMP  requirement  R149. 


3.3.3.2.10.1.2  LOCAL  ENTITIES  DESIGN 
None. 


3.3.3.2.10.1.3  iNPUT/OUTPUT 
GENERIC  PARAMETERS: 

Data  types: 

The  following  table  summarizes  the  generic  formal  data  types  required  by  this 
part: 


Name 

|  Base  Type 

Description 

State  Indices 

(discrete 

1 

Index  to  the  arrays  which  depend 
on  the  number  of  states 

Measuremen  t_Ind ices 

1 

(discrete 

1 

Index  to  the  arrays  which  depend 
on  the  number  of  measurements 

Kalman_Filter_ 

Elements 

1 

| floating 
j  point  type 
( 

| private 

Elements  contained  in  the 

Kalman  Filter  aggregates 

P_Matrices 

Data  type  of  P  matrix 

Measuremen t_ 
Variance_Vectors 

1 

(vector 

1 

1 

Vector  indexed  by  Measurement 
Indices  containing  Kalman_Filter_ 
Elements 

K_Column_Vectors 

1 

(vector 

1 

j 

Vector  indexed  by  State_Indices 
containing  Kalraan_Filter_Elements 

Compact_H_Mat rices 

I 

| vector 

Data  type  of  Compact  H  matrix 

Subprograms : 

The  following  table  suiumarizes  the  generic  formal  subroutines  required  by  this 
part: 


|  Name 

1  . 

1  Type 

...  | 

j  Description  j 

1  . . ...  ....  1 

1 

|  Element 

-  |  ^ 

| function 

1  I 

(Extracts  an  element  of  a  P  Matrix  j 
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3.3.3.2.10.1.4  LOCAL  DATA 


None. 


3.3.3.2.10.1.5  PROCESS  CONTROL 
Not  applicable. 


3.3.3.2.10.1.6  PROCESSING 

The  following  describes  the  processing  performed  by  this  part: 


separate  (Kalman_Filter_Compact_H_Parts) 
function  Compute_Kalman_Gain 


(P 

Measuremen  t_Numbe  r 
Compact_H 

Measuremen t_Variance 
return  K  Column  Vectors 


:  P_Matrices; 

:  Measurement_Indices; 

:  Compact_H_Matrices; 

:  Measurement_Variance_Vectors) 
is 


J  :  State_Indices; 

K  :  K_Column_Vectors; 

begin 


J  :=  Compact_H  (Measuremen t_Number); 


for  I  in  State_Indices  loop 


K  (I)  :=  Element  (P,  I,  J)  / 

(Elenent  (P,  J,  J)  +  Measurement_Variance  (Measurement_Number)) ; 


end  loop; 
return  K; 

end  Compute_Kalman_Gain; 


3.3.3.2.10.1.7  UTILIZATION  OF  OTHER  ELEMENTS 
None. 


3.3.3.2.10.1.8  LIMITATIONS 


None. 


3.3.3.2.10.2  UPDATE_ERROR_COVARIANCE_MATRIX  UNIT  DESIGN  (CATALOG  #P139-0) 

This  unit  is  a  generic  procedure  body  which  computes  the  updated  covariance 
matrix  resulting  from  the  processing  of  a  single  component  of  the  measurement 
vector,  Z. 
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3.3.3.2.10.2.1  REQUIREMENTS  ALLOCATION 
This  part  meets  CAMP  requirement  R150. 

3.3.3.2.10.2.2  LOCAL  ENTITIES  DESIGN 
None. 

3.3.3.2.10.2.3  INPUT/OUTPUT 
GENERIC  PARAMETERS: 

Data  types: 

The  following  table  summarizes  the  generic  formal  data  types  required  by  this 
part: 


|  Name 

1 

Base  Type 

|  Description 

1 

|State_Indices 

1 

discrete 

j Index  to  the  arrays  which  depend 
|  on  the  number  of  states 

i 

i 

| Measurement_Indices 

discrete 

i 

| Index  to  the  arrays  which  depend 
j  on  the  number  of  measurements 

1 

|Kalman_Filter_ 
j  Elements 

1 

floating 
point  type 

1 

| Elements  contained  in  the 
j  Kalman  Filter  aggregates 

1 

1 

|P_Matrices 

private 

1 

(Data  type  of  P  matrix 

1 

1 

1 P_Row_Vectors 

vector 

j 

| Vector  indexed  by  State_Indices 
j  containing  Kalman_Filter_Elements 

|K_Column  Vectors 

1 

vector 

! 

| Vector  indexed  by  State_Indices 
j  containing  Kalman_Filter_Elements 

1 

J  Compact_H_Matrices 

vector 

1 

|Data  type  of  Compact  H  matrix 

Subprograms : 

The  following  table  summarizes  the  generic  formal  subroutines  required  by  this 
part: 
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Name 

|  Type 

i  Description 

|Row 

j  function 
| 

j Extracts  a  row  of  a  P  matrix 

|  »*» 

j 

j  function 

1 

1 

|A  K  Column  Vector  is  multiplied 
j  by  the  transpose  of  a  P  Row  vector, 
j yielding  a  P  matrix 

1 

ft _ If 

J 

1 

| function 

i 

! 

|Two  P  matrices  are  added,  yielding  a 
j Symmetric  matrix 

3.3.3.2.10.2.4  LOCAL  DATA 
None. 


3.3.3.2.10.2.5  PROCESS  CONTROL 
Not  applicable. 


3.3.3.2.10.2.6  PROCESSING 

The  following  describes  the  processing  performed  by  this  part: 

separate  { Kaiman  F i 1 1 er_Compac t_H_Par t s ) 

procedure  Update  Error_Covariance_Matrix 

J?  :  in  out  P  Matrices; 


Measurement  Number 

:  in 

Measurement_Indices ; 

K 

:  in 

K_Column_Vectors ; 
Compact_H_Matrices)  is 

Compact_H 

:  in 

J  :  State_Indices; 
begin 

J  :=  Compact_H  (Measurement_Number); 
P  :=  P  -  K  *  Row  (P,  J); 
end  Update_Error_Covariance_Matrix; 


3.3.3.2.10.2.7  UTILIZATION  OF  OTHER  ELEMENTS 
None. 

3.3.3.2.10.2.8  LIMITATIONS 


None. 
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3.3.3.2.10.3  UPDATE_STATE_VECTOR  UNIT  DESIGN  (CATALOG  #P140-0) 

This  unit  is  a  generic  procedure  body  which  updates  the  State  Vector,  X,  given 
the  old  X  vector,  the  Z  vector,  the  K  vector,  the  Measurement  Number,  and  the 
Compact  H  array. 


3.3.3.2.10.3.1  REQUIREMENT’S  ALLOCATION 
This  part  meets  CAMP  requirement  R152. 


3.3.3.2.10.3.2  LOCAL  ENTITIES  DESIGN 
None. 


3.3.3.2.10.3.3  INPUT/OUTPUT 
GENERIC  PARAMETERS: 

Data  types: 

The  following  table  summarizes  the  generic  formal  data  types  required  by  this 
part: 


|  Name 

Base  Type 

Description 

| State  Indices 

1 

| 

discrete 

Index  to  the  arrays  which  depend 
on  the  number  of  states 

1 

Measurement_Indices 

discrete 

Index  to  the  arrays  which  depend 
on  the  number  of  measurements 

Kalman  Filter_ 
Elements 

floating 
point  type 

Elements  making  up  the 

Kalman  Filter  aggregates 

Measurement_Vectors 

vector 

Vector  indexed  by  Measurement_ 
Indices  containing  Kalman_Filter_ 
Elements 

K_Column_Vectors 

vector 

Vector  indexed  by  State_Indices 
containing  Kalman_Filter_Elements 

State_Vectors 

vector 

Vector  indexed  by  State_Indices 
containing  Kalman_Filter_Elements 

Compact  H_Matrices 
Vectors 

vector 

Vector  indexed  by  Measurement_ 
Indices  containing  State_Indices 

Subprograms: 

The  following  table  summarizes  the  generic  formal  subroutines  required  by  this 
part: 
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Name 

!  Type 

j  Description 

l!  +  » 

j  function 

1 

I 

j  Add  a  state  vector  and  a  K  column 
j vector,  yielding  a  state  vector 

1 

»*» 

1 

|  function 

1 

1 

| Multi ply  a  K  column  vector  by  a  Kal- 
jman  Filter  Element,  yielding  a  K 
j column  vector 

3.3.3.2.10.3.4  LOCAL  DATA 


None. 


3.3.3.2.10.3.5  PROCESS  CONTROL 


Not  applicable. 


3.3.3.2.10.3.6  PROCESSING 

The  following  describes  the  processing  performed  by  this  part* 


separate  (Kalman_Filter_Compact_H_Parts) 
procedure  Update  State  Vector 
<X" 
z 

K 

Measurement_Number 
Compact  H 


in  out  State_Vectors; 
in  Heasurement_Vectors; 
K_Column_Vectors ; 
Measurement_Indices ; 
Compact  H  Matrices)  is 


in 

in 

in 


J  :  State_Indices; 
begin 

J  :=  Compact_H  (Measureraent_Number) 5 
X  :=  K  *  (Z  (Measurement_Number)  -  X  (J))  +  X; 
end  Update_State_Vector; 


3.3.3.2.10.3.7  UTILIZATION  OF  OTHER  ELEMENTS 


None. 


3.3.3.2.10.3.8  LIMITATIONS 


None. 
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3.3.3.2.10.4  UPDATE_ERROR_COVARIANCEJ'-rr.:X_GENERAL_FORM  UNIT  DESIGN  (CATALOG 
#P1121-0) 

This  unit  is  a  generic  procedure  body  which  computes  the  updated  covariance 
matrix  resulting  from  the  processing  of  a  single  component  of  the  measurement 
vector,  Z. 


3.3.3.2.10.4.1  REQUIREMENTS  ALLOCATION 
This  part  meets  CAMP  requirement  R150. 


3.3.3.2.10.4.2  LOCAL  ENTITIES  DESIGN 
None. 


3.3.3.2.10.4.3  INPUT/OUTPUT 
GENERIC  PARAMETERS: 

Data  types: 

The  following  table  summarizes  the  generic  formal  data  types  required  by  this 
part: 
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j  Name 

1  __ 

Base  Type 

Description 

1 

| State_Indices 

i 

discrete 

Index  to  the  arrays  which  depend 
on  the  number  of  states 

i 

(Measurement  Indices 

discrete 

Index  to  the  arrays  which  depend 
on  the  number  of  measurements 

1 

| Kalman  Filter_ 
j  Elements 

floating 
point  type 

Elements  contained  in  the 

Kalman  Filter  aggregates 

1 

|P  Matrices 

1 

private 

Type  of  covariance  matrix.  It 
represents  a  symmetric  matrix 
index  by  (states,  states) 

|K  H  Product 
j  Matrices 

1 

1 

private 

A  matrix  of  the  form  I_KH,  where 

K  is  a  K_Column_Vector  and  H  is 
a  row  of  the  H  matrix.  It 
represents  a  matrix  indexed  by 
(states,  states) 

(Measurement 
j  Variance 

vector 

Type  of  Measurement  Variance 
Vector  (R).  It  is  indexed  by 
(states) 

! 

| K_Column_Vec  tors 

1 

1 

vector 

Type  of  a  Column  of  the  Kalman 
Gain  Matrix  (K).  It  is  indexed 
by  (states) 

! 

| Compact_H_Matrices 

1 

! 

i 

! 

1 

vector 

A  vector,  index  by  Measurement 
Indices  containing  the  indices  of 
the  measured  states.  It  repre¬ 
sents  a  matrix  indexed  by  j 

(Measurement,  states)  that  is  allj 
zeroes  except  for  locations  | 

specified  by  row=I,  column=  j 

Compact_H_Matrices(I)  ( 

Subprograms : 

The  following  table  summarizes  the  generic  formal  subroutines  required  by  this 
part: 
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|  Name 

1  Type 

|  Description  | 

j I_Minus_Column_ 
j  Matrix 

1 

I 

j  function 

1 

! 

I 

|K  and  the  current  state  measured  (as  j 
j indicated  by  whicvh  element  of  the  j 
(current  row  of  H  is  a  "1")  =>  I  KH  j 

|A  B  A  Transpose 

1 

1 

1 

| function 

1 

1  1 

|K  H  Product  Matrices  *  Kalman_Filter  | 
(Elements  *  transpose(K_H  Product  j 

(Matrices)  =>  P_Matrices  j 

I 

|A  B  A  Transpose 

I 

1 

| function 

1 

I 

j 

1  ! 

|K  ColunmJ/ectors  *  Kalman_Filter_  | 

j Elements  *  transpose(  K_Column_  j 

(Vectors)  =>  P_Matrices  | 

|"+" 

1 

j  function 

1  i 

|P_Matrices  +  P_Matrices  =>  P_Matrices| 

3.3.3.2.10.4.4  LOCAL  DATA 


None. 


3.3.3.2.10.4.5  PROCESS  CONTROL 


Not  applicable. 


3.3.3.2.10.4.6  PROCESSING 

The  following  describes  the  processing  performed  by  this  part: 

separate  (Kalman_Filter_Compact_H_Parts) 

procedure  Update_Error_Covariance_Matrix_General_Form 
(P  “  :  in  out  P  Matrices; 


Measurement  Number 

:  in 

Measurement  Indices; 

K 

:  in 

K_Column_Vectors ; 

Compact  H 

:  in 

Compact_H_Matrices; 

Measurement  Variance 

:  in 

Measurement  Variance  Vectors)  is 

IJlinus_K_H_Matrix  :  K_H_Product_Matrices; 

J  :  State_Indices; 

begin 

J  :=  Compact_H  (Measurement_Number) ; 

I_Minus_K_H_Matrix  I_Minus_Column_Matrix  (K  ->  K, 

State  Measure  =>  J); 


P  :=  ABA_Transpose  (I_Minus_K_H_Matrix,  P)  + 

ABA_Transpose  (K,  Measurement_Variance(  Measurement_Number  )); 

end  Update_Error_Covariance_Matrix_General_Form; 
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3.3.3.2.10.4.7  UTILIZATION  OF  OTHER  ELEMENTS 


None. 


3.3.3.2.10.4.8  LIMITATIONS 


None 
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package  body  Kalman_Filter_Compact_H_Parts  is 


function  Compute_Kalman_Gain 

(P  :  P_Matrices; 

Measurement_Number  :  Measurement_Indices; 

Compact_H  :  Compact_H_Ma trices; 

Measurement_Variance  :  Measurement_Variance_Vectors) 

return  K_Column_Vectors  is  separate; 


procedure 


procedure 


Uodate  Error  Covariance  Matrix 


(P  T 
Measu.rement_Number  : 
K  : 
Compact_H  : 
is  separate; 

Update_State_Vector 

(X  : 
Z  : 
K  : 
Measurement_Number  s 
Corapact_H  : 
is  separate; 


in  out  P_Matrices; 
in  Measurement_Indices; 

in  K_Column_Vectors; 

in  Compact_H_Matrices) 


in  out  State_Vectors; 
in  Measurement_Vectors; 

in  K_Coluirn_Vectors; 

in  Measurement_Indices; 

in  Compact_H_Mat rices) 


package  body  Sequentially_Update_Covariance_Matrix_-And_State_Vector 
is  separate; 


package  body  KalmanUpdate  is  separate; 

procedure  Update_Error_Covariance_Matrix_General_Form 


(P 

:  in  out 

P_Matrices; 

Measurement  Number 

:  in 

Measurement_Indices ; 

K 

:  in 

K_Column_Vectors ; 

Compact  H 

:  in 

Compact_H_Ma trices; 

Measurement_Variance 

:  in 

Measurement_Variance_Vectors) 

is  separate; 

end  Kalman_Filter_Compact_H_Parts ; 
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separate  ( Kalman_Fi 1 1 er_Compac t_H_Par t s ) 
function  Compute_Kalman_Gain 

(P  :  P_Matrices; 

Measurement_Number  :  Measurement_Indices; 

Compact_H  :  Compact_H_Matrices; 

Measurement  Variance  :  Measurement_Variance_Vectors) 
return  K_Column_Vectors  is 

J  :  State_Indices; 

K  :  K_Column_Vectors; 

begin 

J  :=  Compact_H  (Measurement_Number); 
for  I  in  State_Indices  loop 
K  (I)  :=  Element  (P,  I,  J)  / 

(Element  (P,  J,  J)  +  Measurement_Variance  (Measurement_Number)) ; 


end  loop; 
return  K; 


end  Compute_Kc»lman_Gain; 
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separate  (Kalman_Fi 1 ter_Compac t_H_Par t s ) 

procedure  Update_Error_Covariance_Matrix 

(P  :  in  out  P  Matrices; 


Measurement  Number 

:  in 

Measurement_Indices ; 

K 

:  in 

K_Column_Vectors; 

Compact_H 

:  in 

Compact_H_Ma trices)  is 

J  :  State  Indices; 


begin 

J  :=  Compact_H  (Measurement_Nunber); 
P  :=  P  -  K  *  Row  (P,  J'* 
end  Update_Error_Covariance_Matrix; 
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separate  (Kalman_Filter_Compact_H 
procedure  Update_State_Vector 
(X 
Z 
K 

Measurement__Number 
Compact_H  ~ 


Parts) 

:  in  out  State_Vectors; 

:  in  Measurement_Vectors 

:  in  K_Column_Vectors; 

:  in  Measurement_Indices 

:  in  Compact_H_Matrices) 


J  :  State_Indices; 
begin 

J  :=  Compact_H  (Measurement_Number) ; 


X  :=  K  *  (Z  (Measurement_Number)  -  X  (J))  +  X; 


is 


end  Update_State_Vector; 
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separate  ( Kalman_Fi 1 1 er_Compac t_H_Par ts ) 

package  body  Sequentially_Update_Covariance_Matrix_And_State_Vector  is 
K  :  K  Column  Vectors; 


function  ComputeJC  is  new  Compute_Kalman_Gain 

(State_Indices  =>  State_Indices, 

Measurement_Indices  =>  Measurement_Indices, 

Kalman_Filter_Elements  =>  Kalman_Filter_Elements, 

P_Ma trices  =>  P_Matrices, 

Measurement_Variance_Vectors 

=>  Measurement_Variance_Vectors, 
K_Column_Vectors  =>  K_Column_Vectors, 

Compact_H_Matrices  =>  Compact_H_Matriees); 

procedure  Update_P  is  nev  Update_Error_Covariance_Matrix 
(State_Indices  =>  State_Indices, 

Measurement_Indices  =>  Measurement_Indices, 

Kalman_Filter_Elements  =>  Kalman_Filter_Elements, 

P_Matrices  =>  P_Matrices, 

P_R°w_Vectors  =>  P_Rov_Vectors, 

K_Column_Vectors  =>  K_Column_Vectors, 

Compact_H_Matrices  =>  Compact_H_Ma trices); 


procedure  Update  X  is  nev  Update  State  Vector 


(State_Indices  =>" 
Measurement_Indices  => 
Kalman_Filter_Elements  => 
Measurement_Vectors  => 
K_Column_Vectors  => 
State_Vectors  => 
Compact_H_Matrices  => 


procedure  ifpdate 

(P  : 

X  : 

Z  : 

Corapact_H  : 

Measurement_Variance  : 

begin 


StateJ'ndices, 

Measurement_Indices , 
Kalman_Filter_Elements , 
Measurement_Vectors , 

K_Column_Vectors , 

State_Vectors, 

Compact_H_Matrices) ; 

pragma  PAGE; 

in  out  P_Matrices; 
in  out  State_Vectors; 
in  Measurement_Vectors; 
in  Compact  H_Matrices; 
in  Measureinent_Variance_Vectors)  is 


for  Measurement_Number  in  Measurement_Indices  loop 

K  :=  ComputeJC  (P  =>  P, 

Measurement  JJumber  =>  Measurement_Number, 

Compact_H  =>  Compact_H, 

Measurement_Variance  =>  Measurement_Variance) ; 

Update_P  (P  =>  P, 

MeasurementJJumber  =>  Measurement  Number, 

K  =>  K, 

Compact_H  =>  Compact_H); 


t VrMi ixK-Xn east?! tv if. tvtjOv  •v. iu Ovx v.  f'vJv, tv  tv. tv  tv.  tv rv.  tsv. nx fix njs fix farx-s V>mjm mx 
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Update_X  (X  =>  X 

Z  =>  Z 

K  =>  K 

Measurement_Number  =>  M< 

Compact_H  =>  Ci 


end  loop; 
end  Update; 

end  Sequentially_Update_Covariance_Matrix 
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with  Kalman_Filter_Common_Parts; 
separate  ( Kalman_Fi 1 ter_Compac  t_H_Par ts ) 
package  body  Kalman_Update  is 

package  P_Manager  is  new 

Kalman_Filter_Common_Parts.Error_Covariance_Matrix_Manager 
(Phi_Matrices  =>  Phi_Matrices, 

P_And_Q_Matrices  =>  P_And_Q_Matrices) ; 

package  Update_P_And_X  is  new 

Sequent ially_Update_Covariance_Matrix_And_State_Vector 
(State_Indices  =>  State_Indices, 

Measurement_Indices  =>  Measurement_Indices, 

Kalman_Filter_Elements  =>  Kalman_Filter_Elements, 

P_Matrices  =>  P_And_Q_Mat rices, 

Measurement_Variance_Vectors 

=>  Measurement_Variance_Vectors, 
Measurement_Vectors  =>  Measureraent_Vectors, 

P_Row_Vectors  =>  P_Rov_Vectors, 

K_Column_Vectors  =>  K_Column_Vectors, 

State_Vectors  =>  State_Vectors, 

Compact_H_Matrices  =>  Compact_H_Ma trices) ; 

Zero  State  Vector  s  State  Vectors  :=  (others  =>  0.0); 


procedure  Update 


begin 


pragma  PAGE; 


(X 

• 

• 

in  out 

State_Vectors; 

p 

• 

• 

in  out 

P_And_Q_Ma  trices ; 

z 

• 

• 

in 

Measurement_Vectors ; 

Compact_H 

• 

• 

in 

Compact_H_Matrices; 

Measurement_Variance 

• 

• 

in 

Measurement_Variance_Vectors 

Propagated_Phi 

• 

• 

in 

Phi  Matrices; 

Propagated_Q 

• 

• 

in 

P_And_Q  Matrices)  is 

—  Propagate  the  Error  Covariance  Matrix 


PManager. Initialize  (Initial_P  =>  P); 

P_Manager. Propagate  (Propagated_Phi  =>  Propagated_Phi, 

Propagated_Q  =>  Propagated_Q) ; 


P  :=  P_Manager.P; 


—  If  the  state  vector  is  not  zero,  multiply  it  by  Propagated  Phi 
if  X  /=  Zero_State_Vector  then 
X  :s=  Propagated_Phi  *  X; 
end  if; 


—  Update  Error  Covariance  Matrix  and  State  Vector 
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separate  (Kalman_Filter_Compact_H_Parts) 


procedure  Update_Error_Covariance_Matrix_General_Form 
(P  :  in  out  PMatrices; 

Measurement_Number  :  in 

K  :  in 

Compact_H  :  in 

Measurement  Variance  :  in 


Measurement_Indices; 
K_Column_Vectors ; 
Compact_H_Matrices; 

Measurement  Variance  Vectors)  is 


I_Minus_K_H_Matrix  :  K_H_Product_Mat rices; 
J  :  State  Indices; 


begin 

J  :=  Compact_H  (Measurement_Number); 

I_Minus_K_H_Matrix  :=  I_Minus_Column_Matrix  (K  =>  K, 

State  Measure  =>  J); 


P  :=  Aba_Transpose  (I_Minus_K_H_Matrix,  P)  + 

Aba_Transpose  (K,  Measurement_Variance(  Measurement_Number  )); 

end  Update_Error_Covariance_Matrix_General_Form; 


v’V.V 
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3. 3. 3. 3  KALMAN  FILTER  COMPLICATED  H  PARTS  (BODY)  TLCSC  (CATALOG  #P149-0) 

This  part,  which  is  designed  as  an  Ada  package,  contains  bodies  for  all  CAMP 
parts  which  can  be  used  to  implement  a  Kalman  Filter  when  a  complicated 
Measurement  Sensitivity  Matrix  (Complicated  H  Matrix)  is  used 

The  decomposition  for  this  part  is  the  same  as  that  shown  in  the  Top-Level 
Design  Document. 

3. 3. 3. 3.1  REQUIREMENTS  ALLOCATION 

The  following  chart  summarizes  the  allocation  of  CAMP  requirements  to  this 
Tlcsc: 


j  Name  |  Requirements  Allocation  | 


(Compute  Kalman  Gain 

|  R182 

1 

j Update  Error  Covariance  Matrix 

j  R183 

1 

j Update  State  Vector 

|  R184 

1 

Sequentially  Update  Covariance  Matrix  and 

j  R201 

j  State  Vector 

1 

1 

j Kalman  Update 

|  R181 

j Update  Error  Covariance  Matrix  General  Form 

|  R183 

1 

3. 3. 3. 3. 2  LOCAL  ENTITIES  DESIGN 
None. 

3. 3. 3. 3. 3  INPUT/OUTPUT 
None. 

3. 3. 3. 3. 4  LOCAL  DATA 
None. 

3. 3. 3. 3. 5  PROCESS  CONTROL 
Not  applicable. 

3. 3. 3. 3. 6  PROCESSING 

The  following  describes  the  processing  performed  by  this  part: 
package  body  Kalman_Filter_Complicated_H_Parts  is 


function  Compute_Kalman_Gain 

(  P 


:  P  Matrices; 
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Measurement_Number  :  Measurement_Indices; 
Complicated_H  :  H_Matrices; 

Measurement_Variance  :  Measurement_Variance_Vectors) 
return  K_Column_Vectors  is  separate; 


procedure  Update_Error_Covariance_Matrix 


(P 

in  out 

P_Matrices; 

Measurement  Number 

in 

Measurement  Indices; 

K 

• 

in 

K_Column_Vectors ; 

Complicated_H 

• 

• 

in 

H_Matrices)  is  separate 

procedure  Update  State  Vector 

(X 

in  out 

State_Vectors; 

Z 

in 

Measurement_Vectors ; 

K 

in 

K_Column  Vectors; 

Measurement  Number 

in 

Measurement  Indices; 

Complicated_H 

in 

H_Matrices)  is  separate 

package  body  Sequentially_Update_Covariance_Matrix_and_State_Vector  is  separate; 
package  body  Kalman_Update  is  separate; 


procedure  Update  Error  Covariance  Matrix  General  Form 


(  P  : 

Measurement_Number  : 

K  : 

Complicated_H  : 

Measurement_Variance  : 

is  separate; 

end  Kalman_Filter_Complicated_H_Parts; 


in  out  P_Matrices; 
in  Measurement_Indices; 

in  K_Column_Vectors; 

in  H_Matrices; 

in  Measurement_Variance_Vectors  ) 


3. 3. 3. 3. 7  UTILIZATION  OF  OTHER  ELEMENTS 
None. 

3. 3. 3. 3. 8  LIMITATIONS 
None. 


3. 3. 3. 3. 9  LLCSC  DESIGN 

3. 3. 3. 3. 9.1  SEQUENTIALLY  UPDATE  COVARIANCE  MATRIX  AND  STATE  VECTOR  PACKAGE  DESIGN 
(CATALOG  #P153-0) 

This  LLCSC  is  a  generic  package  which  contains  1  procedure,  "Update",  which 
updates  the  Covariance  Matrix,  P,  and  state  Vector,  X. 

The  decomposition  for  this  part  is  the  same  as  that  shown  in  the  Top-Level 
Design  Document. 
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3. 3. 3. 3. 9. 1.1  REQUIREMENTS  ALLOCATION 
This  part  meets  CAMP  requirement  R152. 


3. 3. 3. 3. 9. 1.2  LOCAL  ENTITIES  DESIGN 


Packages : 

As  described  above,  this  package  body  instantiates  Compute  Kalman  Gain,  Update 
Error  Covariance  Matrix,  and  Update  State  Vector 


3. 3. 3. 3. 9. 1.3  INPUT/OUTPUT 
GENERIC  PARAMETERS: 

Data  types: 

The  following  table  describes  the  generic  formal  types  required  by  this  part: 


I 
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j  Name 

(  Base  Type 

|  Description  ( 

|States_Indices 

| 

(discrete 

1 

1 

(Index  to  the  arrays  which  depend 
j  on  the  number  of  states 

i 

1 

(Measurement  Indices 

(discrete 

1 

1 

1 

(Index  to  the  arrays  which  depend 
j  on  the  number  of  measurements 

1 

| Kalman  Filter 

1 

| floating 

1 

(Elements  contained  in  the 

j  Elements 

1 

j  point  type 

1 

|  Kalman  Filter  aggregates 

i 

1 

|P_Ma trices 

l 

| private 

i 

1 

|Data  type  for  n  x  n  private  P 
j matrix 

l 

1 

|H  Matrices 

1 

| private 

1 

J 

1 

|Data  type  for  private  m  x  n  H 
j  matrix 

i 

|K  H  Product 
j  Matrices 

1 

( private 

1 

l 

i 

|Data  type  for  private  n  x  n  K 
jand  H  matrix 

1 

|H  Row  Vectors 

1 

| private 

1 

| Vector  representing  a  row  of  a 
jp  matrix 

(Measurement  Vectors 

1 

1 

1 

| vector 

1 

1 

1 

| Vector  indexed  by  Measurement 
j Indices  containing  Kalman  Filter_ 
(Elements 

1 

|  Measurement 
j  Variance_ 

j  Vectors 

1 

1 

(vector 

1 

1 

| Vector  indexed  by  Measurement_ 
j Indices  containing  Kalman_Filter_ 

j  Elements 

1 

| K_Column_Vectors 

1 

| vector 

1 

1 

1 

| Vector  indexed  by  State_Indices 
(containing  Kalman  Filter  Elements 

| State_Vectors 

1 

[vector 

i 

1 

| Vector  indexed  by  State_Indices 
(containing  Kalman  Filter_Elements 

Subprograms : 

The  following  table  summarizes  the  generic  formal  subroutines  required  by  this 
part: 
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|  Name 

1  Type 

|  Description  j 

| Active_H_Vector 

| function 

1 

| Returns  the  row  of  an  (HxN)  H  matrix  | 
j  that  is  referenced  ! 

1  i 

1 

| Subtract_From_ 
i  Identity 

i 

j  procedure 

1 

l 

1  I 
(Subtracts  a  K  and  H  Product  matrix  j 
jfrom  the  Identity  Matrix(i.e.,  I  —  S) j 

1  l 

| Times  Transpose 

1 

1 

1 

| function 

1 

1 

1 

1  1 

(Multiplies  a  P  matrix  by  the  trans-  | 

(pose  of  a  H  Row  Vector,  yielding  a  | 

|K  Column  Vector  ( 

i  i 

»*» 

1 

1 

1 

| function 

1 

1 

I 

1  1 

(Multiplies  a  H  Row  Vector  by  a  K  j 

j Column  vector,  yielding  a  Kalman  j 

| Filter  Element 

1  1 

1 

ii*n 

1 

1 

1 

1 

| function 

i 

i 

i 

1  ! 

(Multiplies  a  H  Row  Vector  by  a  State  | 

(vector,  yielding  a  Kalman  Filter  j 

| Element  j 

1  1 

tt*n 

1 

1 

1 

1 

| function 

1 

1 

f 

!  1 

(Multiplies  a  K  Column  Vector  by  a  H  | 

|Row  Vector,  yielding  a  K  and  H  j 

j Product  Matrix  j 

i  i 

1 

j  *»*" 

1 

I 

1 

| function 

1 

1 

1  1 
(Multiplies  a  K  and  H  Product  Matrix  j 
| by  a  P  Matrix,  yielding  a  P  Matrix  | 

1 

j 

1 

1 

1 

| function 

1 

1  ! 

(Multiplies  a  K  Column  Vector  by  a  | 

| Kalman  Filter  Element,  yielding  a  j 

|K  Column  Vector  j 

1  1 

1 

|"/" 

1 

1 

| 

1 

| function 

1 

1 

1 

I  ! 
(Divides  a  K  Column  Vector  by  a  Kalman | 
(Filter  Elfment,  yielding  a  K  Column  j 
j Vector  j 
i  : 

1 

|  "  +  " 

1 

1 

| function 

1 

|Adds  a  State  Vector  and  a  K  Column  | 
j Vector,  yielding  a  State  Vector  j 

3. 3. 3. 3. 9. 1.4  LOCAL  DATA 
None. 
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package  body  Sequentially_Update_Covariance_Matrix_and_State_Vector  is 
K  :  K  Column  Vectors; 


function  Compute_K  is  new  Compute_Kalman 


(State_Indices  => 

Measurement_Indices  => 

Kalman_Filter_Elements  => 
P_Matrices  => 

H_Hatrices  => 

H_R°w_Vectors  => 

Measurement_Variance_Vectors 

=> 

K_Column_Vectors  =■> 


Gain 

State_Indices, 
Heasurement_Indices, 
Kalman_Filter_Elements , 
P_Matrices, 

H_Matrices, 

H_R°w_Vect°rs, 

Heasureraent_Variance_Vectors 
K  Column  Vectors); 


procedure  Update  P  is  new  Update  Error  Covariance  Matrix 


(State_Indices 
Measurement_Indices 
Kalman_Filter_Elements 
P_Matrices 
H_Matrices 
n_R0w_Vectors 
K_H_Product_Matrices 
K  Column  Vectors 


=>  State_Indices, 

=>  Keasurement_Indices, 

=>  Kalman  Filter_Elements, 
=>  P_MatrIces, 

=>  H_Matrices, 

=>  H_Rov_Vectors, 

=>  KJ!_Product_Matrices, 

=>  K  Column  Vectors); 


procedure  UpdateX  is  new  Update_State_Vector 

(  Stat’e_Indices  =>~State_Indices , 

Measurement_Indices  =>  Measurement_Indices, 

Kalman  Filter_Elements  =>  Kalman  Filter_Elements, 


H_Matrices 
H_R°w_Vectors 
Measurement_Vectors 
K_Column_Vectors 
State  Vectors 


=>  H_Matnces, 

=>  H_Row_Vectors, 

=>  Measurement_Vectors, 
=>  K_Column_Vectors, 

=>  State  Vectors); 


end  Sequentially_Update_Covariance_Matrix_and_State_Vector; 


3. 3. 3. 3. 9. 1.7  UTILIZATION  OF  OTHER  ELEMENTS 
UTILIZATION  OF  OTHER  ELEMENTS  IN  TOP  LEVEL  COMPONENT: 

The  following  tables  describe  the  elements  used  by  this  part  but  defined 
elsewhere  in  the  parent  top  level  component: 

Subprograms  and  task  entries: 

The  following  table  summarizes  the  subroutines  and  task  entries  required  by 
this  part  and  defined  elsewhere  in  the  parent  top  level  component: 
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|  Name 

1  Type 

|  Source 

|  Description  | 

| Compute  Kalman 

| generic 

| parent 

| Computes  the  Kalman  Gain  Vector,  | 

(Gain 

j  function 

1 

I 

1 K  ! 

lUpdate  State 

1 

(generic 

! 

j  parent 

1  ! 

| Updates  the  State  Vector,  X  | 

| Vector 
| 

1  procedure 
i 

1 

1 

i  ! 

lUpdate  Error 

1 

j generic 

| Darant 

1  i 

| Updates  the  Error  Covariance  j 

| Covariance  Matrix | procedure 

1 

| Matrix,  P  j 

3. 3. 3. 3. 9. 1.8  LIMITATIONS 
None. 

3. 3. 3. 3. 9. 1.9  LLCSC  DESIGN 
None. 

3.3.3.3.9.1.10  UNIT  DESIGN 

3.3.3.3.9.1.10.1  UPDATE  UNIT  DESIGN 

This  unit  is  a  procedure  which  does  the  update  of  the  Covariance  Matrix,  P,  and 
the  State_Vector,  X. 

3.3.3.3.9.1.10.1.1  REQUIREMENTS  ALLOCATION 
This  parts  meets  CAMP  Requirement  R201. 

3.3.3.3.9.1.10.1.2  LOCAL  ENTITIES  DESIGN 
None. 

3.3.3.3.9.1.10.1.3  INPUT/OUTPUT 
FORMAL  PARAMETERS: 

The  following  table  describes  this  part's  formal  parameters: 


«rtj .VyvV  V  > >*  «*  £ vv r ji v  «l,% V>  v* jV JV 
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|  Name 

1  Type 

|  Mode 

[  Description  | 

|P 

1 

|P  Matrices 

| in/out 

1 

[Error  Covariance  Matrix  (P)  | 

[to  be  updated  [ 

1  1 

1 

jx 

1 

j  State_Vectors 

1 

j in/out 
1 

1  1 

[State  Vector  (X)  to  be  updatdj 

1  l 

jz 

i 

| Measurement_ 
j  Vectors 

1 

j  in 

1 

1 

1  i 

[Current  Measurement  Vector (Z)| 

1  1 

1  | 

1 

[Complicated  H 

1 

1 

|K  Matrices 

l 

1 

jin 

i 

i 

!  1 

[Current  Measurement  Sensitiv-| 

jity  Matrix  (Complicated  H)  j 

i  1 

[Measurement 
| Variance 

1 

| Measurement 
j  Variance_ 

i 

|  in 

1 

| Current  Measurement  Variance  | 
j Array  j 

3.3.3.3.9.1.10.1.4  LOCAL  DATA 
None. 

3.3.3.3.9.1.10.1.5  PROCESS  CONTROL 
Not  applicable. 

3.3.3.3.9.1.10.1.6  PROCESSING 

The  following  describes  the  processing  performed  by  this  part: 


procedure  Update 

(P 

X 

z 

Complicated_H 

Measurement_Variance 

begin 


in  out  P_Matrices; 
in  out  State_Vectors; 
in  Measurement_Vectors; 
in  H_Matrices; 

in  Measurement  Variance  Vectors)  is 


for  Measurement_Number  in  Measurement_Indices  loop 

K  :=  Compute_K  (P  =>  P, 

Measurement_Number  =>  Measurement_Number, 

Complicated_H  =>  Complicated_H, 

Measurement_Variance  =>  Measurement_Variance) ; 

Update_P  (P  =>  P, 

Measurement_Number  =>  Measurement_Number, 

K  =>  K, 

Complicated_H  =>  Complicated_H) ; 


Update_X  (X 
Z 
K 


=>  X, 

=>  z, 

=>  K, 


Measurement  Number  =>  Measurement  Number, 


a 


i 


/V  h i 


CAMP  Software  Detailed  Design  Document 


Page  389 


Complicated_H  =>  Complicated_H) ; 


end  loop; 
end  Update; 


3.3.3.3.9.1.10.1.7  UTILIZATION  OF  OTHER  ELEMENTS 

UTILIZATION  OF  OTHER  ELEMENTS  IN  TOP  LEVEL  COMPONENT: 

The  following  tables  describe  the  elements  used  by  this  part  but  defined 
elsewhere  in  the  parent  top  level  component: 

Subprograms  and  task  entries: 

The  following  table  summarizes  the  subroutines  and  task  entries  required  by 
this  part  and  defined  elsewhere  in  the  parent  top  level  component: 


|  Name 

1  Type 

Source  |  Description  | 

| Compute  Kalman_ 
|Gain 

| generic 
j  function 

i 

parent 

(Computes  the  Kalman  Gain  Vector,  | 
|K  | 

|Update_State_ 
j Vector 

I 

(generic 
j  procedure 

i 

parent 

j Updates  the  State  Vector,  X  j 

1  1 

t  ) 

| Update_Error_  (generic 

j  Covariance_Matrix  j  procedure 

parent 

| Updates  the  Error  Covariance  | 

j Matrix,  P  I 

Data  types: 

The  following  table  summarizes  the  types  required  by  this  part  and  defined 
elsewhere  in  the  parent  top  level  component: 


W 

I 

IV  ^ 


* >>  *i> .  »>  »>  >>  - 


■i 


) 

\ 

i 


En¬ 
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j  Name 

|  Base  Type 

|  Description  | 

| States  Indices 

(discrete 

1 

| Index  to  the  arrays  which  depend  | 

|  on  the  number  of  states  I 

!  i 

(Measurement  Indices 

|discrete 

1 

!  ! 

| Index  to  the  arrays  which  depend  | 

|  on  the  number  of  measurements  j 

I  Kalman  Filter 

1 

| floating 

1  ! 
(Elements  contained  in  the  j 

|  Elements 

1 

j  point  type 

1 

j  Kalman  Filter  aggregates  j 

1  1 

I 

|P_Mat rices 

l 

1 

(private 

1 

1 

1  1 

(Data  type  for  n  x  n  private  P  ] 

j matrix  J 

l  i 

1 

|H  Matrices 

i 

1 

| private 

I 

i 

1  1 

(Data  type  for  private  m  x  n  H  | 

(matrix  j 

I  E 

1 

(Measurement  Vectors 

1 

| vector 

1 

1 

1  1 

(Vector  indexed  by  Measurement  j 

j Indices  containing  Kalman  Filter  j 

j  Elements  '  j 

1  j 

| Measurement 
j  Variance_ 

S  Vectors 

j vector 

1 

1 

| 

1  1 

(Vector  indexed  by  Measurement  j 

j Indices  containing  Kalman  Filter  j 

j  Elements  ~ j 

I  i 

|K  Column  Vectors 

i 

1 

(vector 

1 

1  1 
| Vector  indexed  by  State_Indices  | 
j  containing  Kalman_Filter_Elementsj 

| State_Vectors 

| vector 

1 

!  1 
| Vector  indexed  by  State_Indices  j 
j  containing  Kalman_Filter_Elements  j 

w 


Data  objects: 

The  following  table  summarizes  the  objects  required  by  this  part  and  defined 
elsewhere  in  the  parent  top  level  component: 


|  Name 

1  Type 

|  Value  |  Description  | 

IK 

1 

] K_Column 
j Vectors 

(variable  | Stores  the  value  of  the  Kalman  j 
i  |Gain  Vector  (K)  j 

3.3.3.3.9.1.10.1.8  LIMITATIONS 
None. 


3. 3. 3. 3. 9. 2  KALMANJJPDATE  PACKAGE  DESIGN  (CATALOG  #P155-0) 

This  LLCSC  is  a  generic  package  which  updates  the  State  Vector,  X,  given  the 
old  X  vector,  the  Z  vector,  the  K  vector,  the  Measurement  Number,  and  the 
Complicated  H  array. 


1 


1 


ix? j>  v  v  v  v<  v  v  v  v  »  _ 


■O  *w  S  CvAlOS. 
"  *  —  *WW 


r  •»  K  % 
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The  decomposition  for  this  part  is  the  same  as  that  shown  in  the  Top-Level 
Design  Document. 


3. 3. 3. 3. 9. 2.1  REQUIREMENTS  ALLOCATION 
This  part  meets  CAMP  requirement  R181. 


3. 3. 3. 3. 9. 2. 2  LOCAL  ENTITIES  DESIGN 
Packages : 

As  mentioned  above,  this  package  instantiates  Sequentially  Update  Covariance 
Matrix  and  State  Vector,  and  Error  Covariance  Matrix  Manager 


3. 3. 3. 3. 9. 2. 3  INPUT/OUTPUT 
GENERIC  PARAMETERS: 


>  r 

S 


(a 


Data  types: 

The  following  table  describes  the  generic  formal  types  required  by  this  part: 


LV.V.v.V.v.8* 
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|  Name 

|  Base  Type 

(  Description  | 

1 

| States  Indices 

(discrete 

(Index  to  the  arrays  which  depend  { 

t 

I 

1 

j  on  the  number  of  states  [ 

1  I 

-3 

1 

(Measurement  Indices 

I 

(discrete 

i  i 

(Index  to  the  arrays  which  depend  | 

\ 

I 

j  on  the  number  of  measurements  j 

1  1 

l 

1 

I Kalman  Filter 

| floating 

1  1 
(Elements  contained  in  the  | 

\ 

T 

- 

|  Elements 

1 

|  point  type 

1 

j  Kalman  Filter  aggregates  i 

1  i 

1 

| Phi  Matrices 

1 

| private 

1  1 
| Data  type  for  private  n  x  n  ?\  i  ( 

I 

j matrix  j 

P  And  Q  Matrices 

| private 

|Data  type  for  n  x  n  private  P  j 

. 

• 

1 

i 

(matrix  j 

5 

|H  Matrices 

1 

| private 

|Data  type  for  private  m  x  n  H  | 

] 

1 

I 

j matrix  j 

I  I 

\ 

K  H  Product 

1 

(private 

|Data  type  for  private  n  x  n  K  j 

£ 

3 

j  Matrices 

i 

(and  H  matrix  j 

i  i 

$ 

1 

|H  Row  Vectors 

| private 

| Vector  representing  a  row  of  a  j 

i 

1 

j  P  matrix  | 

j 

(Measurement  Vectors 

1 

(vector 

1  I 

| Vector  indexed  by  Measurement  j 

i 

j Indices  containing  Kalman  Filter  j 

; 

1 

1 

l 

j Elements  1 

I 

.s 

1 

(Measurement 

1 

(vector 

1  I 

| Vector  indexed  by  Measurement  j 

•  £ 

j 

j  Variance 

i 

j Indices  containing  Kalman  Filter  j 

J 

|  Vectors 

i 

i 

(Elements  j 

i 

|K  Column  Vectors 

(vector 

I  ! 

(Vector  indexed  by  State  Indices  | 

? 

3 

1 

i 

j  containing  Kalman_Filter_Elements  j 

i 

| State  Vectors 

| vector 

1  ! 

(Vector  indexed  by  State  Indices  j 

? 

1 

(containing  Kalman_Filter  Elements! 

$ 

j 

Subprograms: 

The  following  table  summarizes  the  generic  formal  subroutines  required  by  this 
part: 
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L£k£ 


|  Name 

1  Type 

|  Description 

Active_H_Vector 

| function 

1  Returns  the  row  of  an  (MxN)  H  matrix 
j  that  is  referenced 

i 

Subtract  From 
Identity 

| procedure 

1 

1 

1 

(Subtracts  a  K  and  H  Product  matrix 
(from  the  Identity  Matrix(i.e.,  I  -  S) 

1 

Times  Transpose 

1 

| function 

1 

l 

(Multiplies  a  P  matrix  by  the  trans- 
jpose  of  a  H  Row  Vector,  yielding  a 
|K  Column  Vector 

i 

Phi_P_Phi_ 

1 

| function 

I 

1 

l 

1 

(Does  an  ABA  Transpose  opertion  on  a 
( Phi  matrix  and  a  P  and  Q  matrix, 
(yielding  a  P  and  Q  matrix 

1 

"*<» 

1 

| function 

1 

1 

1 

1 

(Multiplies  a  H  Row  Vector  by  a  K 
j Column  vector,  yielding  a  Kalman 
j Filter  Element 

i 

n*ii 

| function 

1 

(Multiplies  a  H  Row  Vector  by  a  State 
j vector,  yielding  a  Kalman  Filter 
j Element 

I 

it*n 

| function 

1 

1 

(Multiplies  a  K  Column  Vector  by  a  H 
(Row  Vector,  yielding  a  K  and  H 
j Product  Matrix 
| 

1 

| function 

i 

i 

1 

(Multiplies  a  K  and  H  Product  Matrix 
| by  a  P  Matrix,  yielding  a  P  Matrix 

1 

»*» 

1 

j  function 

1 

1 

1 

(Multiplies  a  K  Column  Vector  by  a 
j Kalman  Filter  Element,  yielding  a 
|K  Column  Vector 

i)  *u 

1 

| function 

1 

1 

(Multiplies  a  Phi  Matrix  by  a  State 
j Vector  yielding  a  State  Vector 

l 

II  /  ft 

| function 

1 

1 

I 

1 

| Divides  a  K  Column  Vector  by  a  Kalman 
j Filter  Element,  yielding  a  K  Column 

j Vector 

1 

t»  +  ll 

1 

| function 

i 

1 

(Adds  a  State  Vector  and  a  K  Column 
jVector,  yielding  a  State  Vector 

3. 3. 3. 3. 9. 2. 4  LOCAL  DATA 


Data  objects: 

The  following  table  describes  the  data  objects  maintained  by  this  part: 


i 

I 

; 

1 


i 

j 

'■ 


| 


I 


a 

1 
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|  Name 

!  Type 

|  Value  | 

Description  | 

[Zero  State 

|State 

| vector  of | Used  for 

comparsison  to  zero  ] 

jvector 

jvector 

|0  values  jvector 

1 

3. 3. 3. 3. 9. 2. 5  PROCESS  CONTROL 


Not  applicable. 


3. 3. 3. 3. 9. 2. 6  PROCESSING 


The  following  describes  the  processing  performed  by  this  part: 


with  Kalman_Filter_Common_Parts; 

separate  (Kalman_Filter_Complicated_H_Parts) 

package  body  Kalman_Update  is 


package  P_Manager  is  new 

Kalman  Filter  Common_Parts.Error_Covariance_Matrix_Manager 
(  PEi_MatrTces  =>  Phi_Matrices, 

P_And_Q  Matrices  =>  P  And  Q  Matrices  ); 


package 


Update_P_And  X  is  new 

Sequentially  Update_Covariance 
(State_In3ices 
Measuremen  t_Indi ces 
Kalman  Filter_Elements 
P_Ma trices 
H_Matrices 

K_H_Product_Matrices 

H_R°w_Vect°rs 

Measuremen t_Variance_Vec to: 
Measuremen t_Vectors 
K_Column_Vectors 
State  Vectors 


Matrix_And_State_Vector 
=>  State_Indices, 

=>  Measuremen t_Indices, 

=>  Kalman_Filter_Elements, 

=>  P_And_Q_Ma  trices, 

=>  H_Matrices, 

=>  K_H_Product_Matrices, 

=>  H_Row_Vectors, 

s  =>  Measuremen t_Variance_Vectors, 
=>  Measuremen t_Vec tors, 

=>  K_Column_Vectors, 

=>  State  Vectors  ); 


Zero  State  Vector  :  State  Vectors  :=  (others  =>  0.0) ; 


end  Kalman_Update; 


l 


3. 3. 3. 3. 9. 2. 7  UTILIZATION  OF  OTHER  ELEMENTS  j 


The  following  library  units  are  with'd  by  this  part:  1 

1.  Kalman_Filter_Common_Parts  j 

UTILIZATION  OF  EXTERNAL  ELEMENTS: 


Packages : 

The  following  table  summarizes  the  external  packages  required  by  this  part: 


,  -r.  yw 


■T.  r*  r*  vvyv»c« 
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j  Name 

1  Type 

|  Source 

|  Description  j 

1  Error  Covariance 

| generic 

1  Kalman  Filter 

| Manages  the  Covariance  j 

j  Matrix_Manager 

j  package 

| Common  Parts 

| Matrix  (P)  | 

UTILIZATION  OF  OTHER  ELEMENTS  IN  TOP  LEVEL  COMPONENT: 

The  following  tables  describe  the  elements  used  by  this  part  but  defined 
elsewhere  in  the  parent  top  level  component: 

Packages : 

The  following  table  summarizes  the  packages  required  by  this  part  and  defined 
elsewhere  in  the  parent  top  level  component: 


|  Name  |  Type 

|  Source 

|  Description  | 

(Sequentially  | generic 

j  Update  Covariance  j  package 
j Matrix  And  j 

| State  Vector  j 

| parent 

1 

1 

1 

(Updates  the  P  matrix  and  X  Vector) 

I  i 

i  1 

1  1 

I  3. 3. 3. 3. 9. 2. 8  LIMITATIONS 


None. 


3. 3. 3. 3. 9. 2. 9  LLCSC  DESIGN 
None. 


3.3.3.3.9.2.10  UNIT  DESIGN 

3.3.3.3.9.2.10.1  UPDATE  UNIT  DESIGN 

This  unit  is  a  procedure  which  Uo^s  the  Kalman  Update 


3.3.3.3.9.2.10.1.1  REQUIREMENTS  ALLOCATION 
This  parts  meets  CAMP  Requirement  R181. 


3.3.3.3.9.2.10.1.2  LOCAL  ENTITIES  DESIGN 


None. 


^ a  wav.^v.v.wo.v.v  v.v.v.‘aV.v,v;aV,v^.v  v 
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3.3.3.3.9.2.10.1.3  INPUT/OUTPUT 
FORMAL  PARAMETERS: 

The  following  table  describes  this  part's  formal  parameters: 


|  Name 

1  Type 

|  Mode  |  Description  | 

(P 

1 

l 

|P  And  Q_Matriees 

1 

| in/out 

I 

1 

| Error  Covariance  Matrix  (P)  | 

j  to  be  updated  ( 

i  i 

1 

jx 

i 

1 

|State_Vectors 

1 

| in/out 

I 

| State  Vector  (X)  to  be  updatd| 

1  i 

1 

jz 

1 

1 

| Measurement 
| Vectors 

I 

1  in 

| Current  Measurement  Vector(Z) | 

1  1 

1  I 

1 

(Complicated  H 

1 

1 

|H  Matrices 

1 

jin 

1 

1  S 
(Current  Measurement  Sensitiv-| 
jity  Matrix  (Complicated  H)  j 

1  i 

1 

(Measurement 

j Variance 

1 

1 

(Measurement 
j  Variance_Vectors 

1 

j  in 

1  ! 

| Current  Measurement  Variance  | 
j  Array  j 

1  1 

| Propagated  Phi 

1 

| Phi  Matrices 

1 

1 

j  in 

1 

1 

1  1 

| Current  value  of  the  propa-  j 
| gated  State  Transition  Matrix j 

1  1 

1 

| Propagated_Q 

1 

|P  And  Q  Matrices 

|  in 

i  1 

| Current  value  of  the  propa-  1 
j gated  Process  Noise  Matrix  ( 

3.3.3.3.9.2.10.1.4  LOCAL  DATA 
None. 


3.3.3.3.9.2.10.1.5  PROCESS  CONTROL 
Not  applicable. 


3.3.3.3.9.2.10.1.6  PROCESSING 

The  following  describes  the  processing  performed  by  this  part: 


;x 

in  out 

State_Vectors; 

P 

in  out 

P_And  Q  Matrices; 

Z 

in 

Measurement_Vectors; 

Complicated  H 

in 

H_Matrices; 

Measurement  Variance 

in 

Measurement  Variance  Vectors; 

Propagated_Phi 

in 

Phi_Matrices; 

Propagated_Q 

in 

P_And_Q_Matrices)  is 

begin 

—  Propagate  the  Error  Covariance  Matrix 


was 
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P_Manager. Initialize  (Initial_P  =>  P); 

P_Manager. Propagate  (Propagated_Phi  =>  Propagated_Phi, 

Propagated_Q  =>  Propagated_Q); 


P  :=  P_Hanager.P; 

—  If  the  state  vector  is  not  zero,  multiply  it  by  Propagated  Phi 
if  X  /=  Zero_State_Vector  then 

X  :=  Propagated_Phi  *  X; 
end  if; 

—  Update  Error  Covariance  Matrix  and  State  Vector 

Update_P_And_X. Update  (P  =>  P, 

X  =>  X, 

Z  =>  Z, 

Complicated_H  =>  Complicated_H, 

Measurement  Variance  =>  Measurement  Variance); 


end  Update; 


3.3.3.3.9.2.10.1.7  UTILIZATION  OF  OTHER  ELEMENTS 
UTILIZATION  OF  OTHER  ELEMENTS  IN  TOP  LEVEL  COMPONENT: 

The  following  tables  describe  the  elements  used  by  this  part  but  defined 
elsewhere  in  the  parent  top  level  component: 

Packages : 

The  following  table  summarizes  the  subroutines  and  task  entries  required  by 
this  part  and  defined  elsewhere  in  the  parent  top  level  component: 


|  Name 

1  Type 

|  Source 

|  Description  | 

|P  Manager 

1 

1 

1 

| package 

1 

| Kalman 
j  Filter_ 
j  Common 
j Parts 

i 

| Manages  the  Error  Covariance  j 
jmatrix  (it  is  an  instantiation  ofj 
jpart  R146)  j 

1  1 

1 

|Update_P_And_X 

1 

1 

| package 

1 

1 

1 

| parent 

1 

1 

1  1 
[Updates  the  Covariance  Matrix  and| 
|X  Vector  (it  is  an  instantiation  j 

J  of  part  R201)  j 

Data  types: 

The  following  table  summarizes  the  types  required  by  this  part  and  defined 
elsewhere  in  the  parent  top  level  component: 
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|  Name 

|  Base  Type 

|  Description  j 

|States_Indices 

l 

(discrete 

i 

I 

| Index  to  the  arrays  which  depend  | 
j  on  the  number  of  states  j 

l  i 

(Measurement  Indices 

l 

1 

(discrete 

(Index  to  the  arrays  which  depend  | 
j  on  the  number  of  measurements  j 

i  i 

1 

| Kalman  Filter 
|  Elements 

1 

1 

| floating 
j  point  type 

1 

1 

(Elements  contained  in  the  j 

!  Kalman  Filter  aggregates  j 

i  I 

l 

| Phi  Matrices 

jp  And  Q  Matrices 

1 

1 

| private 

1 

| private 

1 

i  1 
|Data  type  for  private  n  x  n  Phi  ( 
(matrix  j 
|Data  type  for  n  x  n  private  P  j 
j matrix  [ 

1  s 

1 

|H  Matrices 

| private 

1 

l 

i  I 

|Data  type  for  private  ra  x  n  H  j 

j matrix  j 

|  1 

|K_H_Product_ 
j  Matrices 

1 

1 

j  private 

1 

| 

! 

(Data  type  for  private  n  x  n  K  j 

jand  H  matrix  j 

1 

1 

|H  Row  Vectors 

1 

| private 

1 

I 

1 

| Vector  representing  a  row  of  a  ( 
jp  matrix  j 

(Measurement  Vectors 

I 

1 

I 

(vector 

i 

i 

!  ! 

| Vector  indexed  by  Measurement  ! 

j Indices  containing  Kalman  Filter  j 
(Elements  ~  j 

I 

1 

|  Measurement 
|  Variance 
|  Vectors 

| vector 

1 

1 

l 

1 

| Vector  indexed  by  Measurement  j 
j Indices  containing  Kalman_Filter_j 
(Elements  j 

1 

|K_Column  Vectors 

1 

1 

(vector 

1 

(Vector  indexed  by  State_Indices  ( 
j  containing  Kalman_Filter_Elements  j 

|State_Vectors 

| vector 

1 

j  1 

| Vector  indexed  by  State  Indices  j 
| containing  Kalman_Filter_Elements  j 

Data  objects: 

The  following  table  summarizes  the  objects  required  by  this  part  and  defined 
elsewhere  in  the  parent  top  level  component: 


|  Name 

1  Type 

|  Value  | 

Description  | 

[Zero  State 

| State 

(vector  of (Used  for 

comparison  to  zero  ( 

(Vector 

j Vectors 

(0  values  | vector 

1 
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3.3.3.3.9.2.10.1.8  LIMITATIONS 
None. 

3.3.3.3.10  UNIT  DESIGN 

3.3.3.3.10.1  COMPUTE_KALMAN_GAIN  (BODY)  UNIT  DESIGN  (CATALOG  #P150-0) 

This  LLCSC  is  a  generic  function  body  which  computes  the  Kalman  gain  vector 
resulting  from  the  processing  of  a  single  component  of  the  measurement  vector, 
Z. 


3.3.3.3.10.1.1  REQUIREMENTS  ALLOCATION 
This  part  meets  CAMP  requirement  R182. 

3.3.3.3.10.1.2  LOCAL  ENTITIES  DESIGN 
None. 

3.3.3.3.10.1.3  INPUT/OUTPUT 
GENERIC  PARAMETERS: 

Data  types: 

The  following  table  describes  the  generic  formal  types  required  by  this  part: 
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|  Name 

(  Base  Type 

|  Description  | 

| State  Indices 

1 

(discrete 

! 

(Index  to  the  arrays  which  depend  | 
j  on  the  number  of  states  j 

i  i 

1 

(Measurement  Indices 

i 

| discrete 

i 

\ 

(Index  to  the  arrays  which  depend  | 

|  on  the  number  of  measurements  j 

i  I 

1 

| Kalman  Filter 
j  Elements 

i 

| floating 
j  point  type 

1 

1  1 

(Elements  contained  in  the  | 

j  Kalman  Filter  aggregates  j 

1  i 

1 

|P  Matrices 

1 

| private 

1 

1  1 

|Data  type  for  n  x  n  P  matrixx  ! 

1  | 

i 

|H  Matrices 

1 

| private 

1 

1  1 

|Data  type  for  n  x  n  H  matrix  j 

j  1 

1 

|H  Rov_Vectors 

i 

1 

(private 

1 

i 

1  1 

|Data  type  of  vector  representing  | 

jan  H  vector  j 

I  l 

(Measurement 
|  Variance_ 

|  Vectors 

1 

( array 

1 

1 

1  1 

| Vector  indexed  by  Measurement  | 

j Indices  containing  Kalman  Filter  j 

(Elements  j 

l  1 

1 

(K  Column  Vectors 

1 

| array 

1 

1  1 
| Vector  indexed  by  State_Indices  | 
(containing  Kalman_F?lter  Elements! 

Subprograms: 

The  following  table  summarizes  the  generic  formal  subroutines  required  by  this 
part: 


|  Name 

1  Type 

|  Description  j 

|Active  H  Vector 

( function 

1 

1 

| Returns  the  row  of  an  (MxN)  H  matrix  | 

| that  is  referenced  j 

i  I 

1 

| Times_Transpose 

1 

j 

1 

| function 

1 

1 

1 

1  1 

(Multiplies  a  P  matrix  by  the  trans-  | 

(pose  of  a  n  x  1  H  Row  Vector,  yield-  j 

j ing  a  K  Column  Vector  i 

1  | 

1 

|Dot_Product 

1 

j 

1 

( function 

1 

1 

(Multiplies  a  (Nxl)  H  Row  Vector  by  a  | 
|K  Column  Vector,  yielding  a  Kalman  j 
(Filter  Element  j 
:  i 

1 

| "/" 

1 

I 

|  function 

1 

! 

!  1 

| Divides  a  (Nxl)  K  Column  Vector  by  a  | 
jKalman  Filter  Element,  yielding  a  | 
(K  Column  Vector  | 

3.3.3.3.10.1.4  LOCAL  DATA 
Data  objects: 


J*W. V**'»^*;  !■**■  "-**  L**»  V*» ’*»•*»  *»■*»  l*-’**1^  ■  «»  L*V  L^>.  L*%»  l*>  *L^  L^» 
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The  following  table  describes  the  data  objects  maintained  by  this  part: 


|  Name 

1  Type  | 

Value 

Description 

1 

j Denominator 

| Kalman  Fil- 

| variable 

(One  element  of  the  Error 

1 

1 

I 

j  ter_Elements 

1 

| 

(Covariance  matrix 

j 

1 

1 

|K 

|K  Column 

(variable 

1 

(Vector  which  is  calculated  and 

1 

I 

1 

1 

[Vectors 

1 

| 

j  returned 

1 

|V 

|H  Row 

1 

(variable 

(Column  slice  of  the  H  matrix 

1 

1 

1 

| Vectors 

1 

1 

1 

(representing  the  current 
(measurement 

1 

1 

3.3.3.3.10.1.5  PROCESS  CONTROL 
Not  applicable. 


3.3.3.3.10.1.6  PROCESSING 

The  following  describes  the  processing  performed  by  this  part: 

separate  (Kalman_Filter_ComplicatedJ!_Parts) 

function  Compute_Kalman_Gain 

(P  :  P_Matrices; 

Measurement_Number  :  Measurement_Indices; 
Complicated_H  :  H_Matrices; 

Measurement_Variance  :  Measurement_Variance_Vectors) 
return  K  Column  Vectors  is 


Denominator  :  Kalman_Filter_Elements; 

K  :  K_Column_Vectors; 

V  :  H_Rov_Vectors; 

begin 

V  :=  Active_H_Vector  (Source  =>  Complicated_H, 

Row  =>  Measureraent_Number) ; 

K  :=  Times_Transpose  (Left  =>  P, 

Right  =>  V); 

Denominator  :=  Dot_Product(Lef t  =>  V, 

Right  =>  K)  + 

Measurement_Variance(  Measurement  Number  ); 
return  K  /  Denominator; 
end  Compute_Kalman_Gain; 
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3.3.3.3.10.1.7  UTILIZATION  OF  OTHER  ELEMENTS 


None. 


3.3.3.3.10.1.8  LIMITATIONS 


None. 


3.3.3.3.10.2  UPDATE_ERROR_COVARIANCE_MATRIX  (BODY)  UNIT  DESIGN  (CATALOG  SP151-0) 


This  unit  is  a  generic  procedure  body  which  computes  the  updated  covariance 
matrix  resulting  from  the  processing  of  a  single  component  of  the  measurement 
vector,  Z. 


3.3.3.3.10.2.1  REQUIREMENTS  ALLOCATION 
This  part  meets  CAMP  requirement  R183. 


3.3.3.3.10.2.2  LOCAL  ENTITIES  DESIGN 


None. 


3.3.3.3.10.2.3  INPUT/OUTPUT 


GENERIC  PARAMETERS: 


Data  types: 


The  following  table  describes  the  generic  formal  types  required  by  this  part: 
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|  Name 

|  Base  Type 

j  Description 

|  State  Indices 

[discrete 

1 

| 

| Index  to  the  arrays  which  depend 
|  on  the  number  of  states 

[Measurement  Indices 

1 

| discrete 

i 

1 

(Index  to  the  arrays  which  depend 
j  on  the  number  of  measurements 

i 

j Kalman  Filter 

j  floating 

i 

[Elements  contained  in  the 

j  Elements 

j  point  type 

1 

j  Kalman  Filter  aggregates 

1 

|P_Ma trices 

| private 

1 

l 

|Data  type  for  private  n  x  n  P 
[matrix 

|H  Matrices 

1 

| private 

1 

|Data  type  for  private  H  matrix 

i 

|H  Row  Vectors 

) private 

1 

1 

| Private  vector  representing  a 
jrow  of  the  fi  matrix 

i 

|K  H  Product 
j  Matrices 

I  array 

1 

1 

[Private  matrix  representing 
j product  of  K  and  H  matrices 

l 

|K  Column  Vectors 

| vector 

i 

1 

| Vector  indexed  by  State  Indices 
[containing  Kalman  Filter  Elements 

Subprograms: 

The  following  table  summarizes  the  generic  formal  subroutines  required  by  this 
part: 


|  Name  |  Type  |  Description 


Returns  the  row  of  a  (MxN)  H  Matrix 
that  is  referenced 

Subtracts  a  K_H_Product  matrix  from 
the  Identity  Matrix  (i.e.,  I  -  S) 

Multiplies  a  K  Column  Vector  by  a  H 
Row  Vector,  yielding  a  K  H  Product 
Matrix 

Multiplies  a  K  H  Product  Matrix  by  a 
P  Mju'rix,  yielding  a  P  Matrix 


3.3.3.3.10.2.4  LOCAL  DATA 
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3.3.3.3.10.2.5  PROCESS  CONTROL 
Not  applicable. 

3.3.3.3.10.2.6  PROCESSING 

The  following  describes  the  processing  performed  by  this  part: 

separate  (Ralman_Filter_Complicated_H_Parts) 

procedure  Update_Error_Covariance_Matrix 

(P  :  in  out  P  Matrices; 


Measurement  Number 

:  in 

Measurement 

Indices; 

K 

:  in 

K  Column  Vectors; 

Complicated_E 

:  in 

H_Matrices) 

is 

V  :  H_Row_Vectors; 
begin 

V  :=  Active_H_Vector  (Source  =>  Complicated_H, 

Row  =>  Measurement_Number) ; 

P  :=  Subtract_From_Identity  (K  *  V)  *  P; 
end  Update_Error_Covariance_Matrix; 

3.3.3.3.10.2.7  UTILIZATION  OF  OTHER  ELEMENTS 
None. 

3.3.3.3.10.2.8  LIMITATIONS 
None. 

3.3.3.3.10.3  UPDATE_STATE_VECTOR  UNIT  DESIGN  (CATALOG  #P152-0) 

This  unit  is  a  generic  procedure  which  updates  the  State  Vector,  X,  given  the 
old  X  vector,  the  Z  vector,  the  K  vector,  the  Measurement  Number,  and  the 
Complicated  H  array. 

3.3.3.3.10.3.1  REQUIREMENTS  ALLOCATION 
This  part  meets  CAMP  requirement  R184. 

3.3.3.3.10.3.2  LOCAL  ENTITIES  DESIGN 
None. 


'*1  &■ 
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3.3.3.3.10.3.3  INPUT/OUTPUT 
GENERIC  PARAMETERS: 

Data  types: 

The  following  table  describes  the  generic  formal  types  required  by  this  part: 


|  Name 

|  Base  Type 

1  Description  j 

|State_Indices 

l 

[discrete 

1 

I 

[Index  to  the  arrays  which  depend  | 
j  on  the  number  of  states  j 

1  I 

i 

[Measurement  Indices 

1 

[discrete 

1 

I 

1  1 

| Index  to  the  arrays  which  depend  | 

|  on  the  number  of  measurements  ■ 

1  I 

|Kalman_Filter 
j  Elements 
| 

| floating 
j  point  type 

1 

1  ! 

[Elements  contained  in  the  j 

j  Kalman  Filter  aggregates  | 

1  1 

1 

|H_Mat rices 

1 

| private 

1 

1  1 

|Data  type  of  m  x  n  H  matrix  j 

:  i 

1 

|H  Row  Vectors 

1 

1 

| private 

l 

| Vector  representing  a  row  of  an  | 
|H  matrix  | 

I 

[Measurement  Vectors 

1 

i 

| vector 

1 

| 

1 

| Vector  indexed  by  Measurement  j 

j Indices  j 

1 

j  K_Column_Vectors 

| 

| vector 

1 

1 

i 

| Vector  indexed  by  State_Indices  [ 
j  containing  Kalman_Filter_Elements  j 

|State_Vectors 

1 

(vector 

i 

i  1 

| Vector  indexed  by  State_Indices  j 
j  containing  Kalman_Filter_Elements  j 

Subprograms : 


The  following  table  summarizes  the  generic  formal  subroutines  required  by  this 
part: 
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|  Name 

!  Type 

|  Description 

1 

|Active  H  Vector  j function 

!  1 

1  1 

j Returns  the  row  of  a  (MxN)  H  matrix 
j  that  is  referenced 

1 

1 

i 

»*« 

1 

1 

| 

l 

| function 

1 

| Computes  the  product  of  an  H  Row 
j vector  and  a  State  vector,  yeilding 
ja  Kalman  Filter  Element 

1 

1 

1 

1 

1 

|Dot  Product 

I 

I 

| function 

i 

i 

| Multiplies  a  (Nxl)  K  Column  Vector 
jby  a  Kalman  Filter  Element,  yielding 
ja  K  Column  Vector 

1 

1 

1 

1 

! 

j "+" 

1 

1 

| function 

1 

1 

|Adds  a  state  ”ector  and  a  K  column 
j vector  (both  nxl)  yielding  a  state 
j vector 

1 

1 

1 

3.3.3.3.10.3.4 

LOCAL  DATA 

Data  objects: 

The  following 

table  describes 

the  data  objects  maintained  by  this  part: 

|  Name 

1  Type  I 

Value  |  Description 

1 

IV 

1 

| H_Rov_  | 

j Vectors  j 

1  1 

variable  | Column  slice  of  the  H  matrix 
j representing  the  current 
j measurement 

3.3.3.3.10.3.5  PROCESS  CONTROL 
Not  applicable. 


3.3.3.3.10.3.6  PROCESSING 

The  following  describes  the  processing  performed  by  this  part: 

separate  (Kalman_Filter_Complicated_H_Parts) 
procedure  Update_State_Vector 

(X  :  in  out  State  Vectors; 


Z 

:  in 

Measurement_Vectors ; 

K 

:  in 

K_Column_Vec  tors ; 

Measurement_Number 

:  in 

Measurement_Indices ; 

Complicated  H 

:  in 

H_Matrices)  is 

V  :  H  Row_Vectors; 
begin 

V  :=  Active_H_Vector  (Source  =>  Complicated_H, 


%  V 


*  -  * ' ^ 


vs 
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Row  =>  Measurement  Number); 


X  :=  K  *  (Z  (Measurement_Number)  - 
Dot_Product(Lef t  =>  V, 

Right  =>  X))  +  X; 


end  Update_State_Vector; 


3.3.3.3.10.3.7  UTILIZATION  OF  OTHER  ELEMENTS 
None. 


3.3.3.3.10.3.8  LIMITATIONS 


None. 


3.3.3.3.10.4  UPDATE_ERR0R_C0VARIANCE  MATRIX  GENERAL  FORM  UNIT  DESIGN  (CATALOG 
IP1119-0) 

This  unit  is  a  generic  procedure  body  which  computes  the  updated  covariance 
matrix  resulting  from  the  processing  of  a  single  component  of  the  measurement 
vector,  Z.  This  solution  uses  the  general  form  of  the  equation. 


3.3.3.3.10.4.1  REQUIREMENTS  ALLOCATION 
This  part  meets  CAMP  requirement  R183. 


3.3.3.3.10.4.2  LOCAL  ENTITIES  DESIGN 
None. 


3.3.3.3.10.4.3  INPUT/OUTPUT 
GENERIC  PARAMETERS: 

Data  types: 

The  following  table  describes  the  generic  formal  types  required  by  this  part: 
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|  Name 

|  Base  Type 

j  Description  | 

State_Indices 

|discrete 

1 

j Index  to  the  arrays  which  depend 

j  on  the  number  of  states 

1 

Measurement  Indices 

(discrete 

1 

1 

j Index  to  the  arrays  which  depend 
j  on  the  number  of  measurements 
| 

Kalman_Filter_ 

Elements 

i floating 
point  type 

! 

j Elements  contained  in  the 
i  Kalman  Filter  aggregates 

1 

P  Matrices 

private 

1 

[Data  type  for  private  n  x  n  P 
j  matrix 

1 

H  Matrices 

private 

i 

|Data  type  for  private  H  matrix 

1 

H  Row  Vectors 

private 

| Private  vector  representing  a 
jrov  of  the  H  matrix 
j 

K_H_Product_ 

Matrices 

array 

1 

| Private  matrix  representing 
j  product  of  K  and  H  matrices 

1 

Measurement 
Variance  Vectors 

array 

1 

| Vector  indixed  by  Measurement 
j Indices  containing  KF  Elements 

I 

K  Column_Vectors 

vector 

! 

| Vector  indexed  by  State_Indices 
j  containing  Kalman_Filter_Elements 

Subprograms : 

The  following  table  summarizes  the  generic  formal  subroutines  required  by  this 
part; 
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Name 

1  Type 

j  Description 

Active_H_Vector 

| function 

1 

| 

(Returns  the  row  of  a  (MxN)  H  Matrix 
| that  is  referenced 

i 

Subtract  From 
Identity 

1 

j  procedure 

1 

1 

1 

(Subtracts  a  K_H_Product  matrix  from 
| the  Identity  Matrix  (i.e.,  I  -  S) 

j 

ABA  Transpose 

1 

| function 

i 

i 

1 

(Does  an  ABA  transpose  on  a  K  H 
| Matrix  and  a  P  Matrix,  yielding  a  P 
| Matrix 

l 

ABA_Transpose 

i 

j  function 

1 

1 

I 

1 

(Does  an  ABA  transpose  on  a  K  Column 
j Vector  and  a  Kalman  Filter  Element, 
j yielding  a  P  Matrix 

1 

«*" 

1 

| function 

i 

i 

1 

(Multiplies  a  K  Column  Vector  by  a  H 
|Row  Vector,  yielding  a  K  H  Product 
|Matrix 

I 

1»  +  ft 

| function 

1 

1 

|Adds  two  P  Matrices,  yielding  a  P 
| Matrix 

3.3.3.3.10.4.4  LOCAL  DATA 
Data  objects: 

The  following  table  describes  the  data  objects  maintained  by  this  part: 


|  Name 

1  Type 

Value 

|  Description 

1 

|  A 

1 

|K_H_Product 
j  Matrices 

variable 

(Product  of  K  and  H  Matrices 

1 

1 

1 

1 

jv 

1 

1 

|  ~ 

| H_Row_ 
j Vectors 

1 

variable 

1 

| Column  slice  of  the  H  matrix 
(representing  the  current 
(measurement 

1 

1 

1 

5 

J, 

A 

{ 

) 

fi 

% 

j 


3.3.3.3.10.4.5  PROCESS  CONTROL 


Not  applicable. 


3.3.3.3.10.4.6  PROCESSING 

The  following  describes  the  processing  performed  by  this  part: 
separate  (Kalman_Filter_Coraplicated_H_Parts) 


procedure  Update_Error_Covariance_Matrix_General_Forra 
(P  ~  :  in  out  P  Matrices; 


v  V  V  V 
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Measurement_Number  :  in  Measurement_Indices; 

K  :  in  K_Column_Vectors; 

Complicated_H  :  in  H  Matrices; 

Measurement_Variance  :  in  Measurement_Variance_Vectors  )  is 

A  :  K_H_Product_Matrices; 

V  :  H_Row_Ve_tors; 

begin 

V  :=  Active_H_Vector  (Source  =>  Complicated_H, 

Row  =>  Measurement_Number) ; 

A  :=  Subtract_Frora_Identity  (K  *  V); 

P  :=  ABA_Transpose(  A,  P  )  + 

ABA_Transpose(  K,  Measurement_Variance(  Measurement_Number  )  ); 
end  Update_Error_Covariance_Matrix_General_Form; 


3.3.3.3.10.4.7  UTILIZATION  OF  OTHER  ELEMENTS 
None. 


3.3.3,3.10.4.8  LIMITATIONS 


CAMP  Software  Detailed  Design  Document 


Page  411 


package  body  Kalman_Filter_Complicated_H_Parts  is 


function  Compute_Kalman  Gain 

(  P  :  P_Matrices; 

Measurement_Number  :  Measurement_Indices; 

Complicated_H  :  H_Matrices; 

Measurement_Variance  :  Measurement_Variance_Vectors) 

return  K  Column_Vectors  is  separate; 


procedure  Update_Error_Covariance_Matrix 

(P  :  in  out  P_Matrices; 

Measurement_Number  :  in  Measurement_Indices; 

K  :  in  K_Column_Vectors; 

Complicated_H  :  in  H_Matrices)  is  separate; 

procedure  Update_State_Vector 

(X  s  in  out  State_Vectors; 

Z  :  in  Measurement_Vectors; 

K  :  in  K_Column_Vectors; 

Measurement_Number  :  in  Measurement_Indices; 

Complicated_H  :  in  H_Matrices)  is  separate; 

package  body  Sequentially_Update_Covariance_Matrix_And_State_Vector  is  separate; 

package  body  Kalman_Update  is  separate; 

procedure  Update_Error_Covariance_Hatrix_General_Form 
(  P  ~  :  in  out  P_Matrices; 

MeasurementJNumber  :  in  Measurement_Indices; 

K  :  in  K_Column_ Vectors; 

Complicated_H  :  in  H_Matrices; 

Measurement_Variance  :  in  Measurement_Variance_Vectors  ) 

is  separate; 


end  Kalman  Filter  Complicated  H  Parts; 
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separate  (Kalman_Filter_Complicated_H_Parts) 

function  Compute_Kalman_Gain 

(P  :  P_Matrices; 

Measurement_Number  :  Measurement_Indices; 
Complicated_H  :  H_Matrices; 

Measurement_Variance  :  Measurement_Variance_Vectors) 
return  K  Column  Vectors  is 


Denominator  :  Kalman_Filter_Elements; 

K  :  K_Column_Vectors; 

V  :  H_Row_Vectors; 

begin 

V  :=  Active_H_Vector  (Source  =>  Complicated_H, 

Row  =>  Measurement_Number) ; 

K  :=  Times_Transpose  (Left  =>  P, 

Right  =>  V); 

Denominator  :=  Dot_Product(Lef t  =>  V, 

Right  =>  K)  + 

Measurement_Variance(  Measurement_Number  ); 
return  K  /  Denominator; 
end  Compute_Kalman_Gain; 


>  * 


?u/Hvr 
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separate  (Kalman_Filter_Complicated_H_Parts) 

procedure  Update_Error_Covariance_Matrix 

(P  :  in  out  P_Matrices; 

Measurement_Number  :  in  Measurement_Indices; 
K  :  in  K_Column_Vectors; 

Complicated_H  :  in  H_Matrices)  is 

V  :  H_Row_Vectors; 
begin 

V  :=  Active_H_Vector  (Source  =>  Complicated_H, 

Row  =>  Measurement_Number) ; 

P  :=  Subtract_From_Identity  (K  *  V)  *  P; 
end  Update_Error_Covariance_Ma  ix; 
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separate  (Kalman_Filter_Complicated_H_Parts) 
procedure  Update_State_Vector 

(X  :  in  out  State_Vectors; 

Z  :  in  Measurement_Vectors 

K  :  in  K_Column_Vectors; 

Measurement_Number  :  in  Measurement_Indices 

Complicated_H  s  in  H_Matrices)  is 

V  :  H_Row_Vectors; 
begin 

V  :=  Active_H_Vector  (Source  =>  Complicated_H, 

Row  =>  Measurement_Number) ; 

X  :=  K  *  (Z  (Measurement  Number)  - 
Dot_Product(LeIt  ->  V, 

Right  =>  X))  +  X; 

end  Update_State_Vector; 
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separate  (Kalman_Filter_Complicated_H_Parts) 

package  body  Sequentially_Update_Covariance_Matrix_And_State_Vector  is 
K  :  K  Column  Vectors; 


function  Compute_K  is  new  Compute_Kalman_Gain 

(State_Indices  =>  State_Indices, 

Measurement_Indices  =>  Measurement_Indices, 

Kalman  Filter_Elements  =>  Kalman  Filter_Elements, 

P_Matrices  =>  P_MatrIces, 

H_Matrices  =>  H_Matrices, 

H_R°w_Vectors  =>  H_Row_Vectors, 

Measurement_Variance_Vectors 

=>  Measurement_Variance_Vectors, 
K_Column_Vectors  =>  K  Column  Vectors); 


procedure  Update_P  is  new  Update_Error_Covariance_Matrix 
(State_Indices  =>  State_Indices, 

Measurement_Indices  =>  Measurement_Indices, 

Kalman  Filter_Elements  =>  Kalman  Filter_Elements, 
P_Matrices  =>  P_MatrTces, 

H_Matrices  =>  H_Matrices, 

H_Row_Vectors  =>  H_Row_Vectors, 

K_H_Product_Matrices  =>  K_H_Product_Matrices, 

K_Column_Vectors  =>  K_Column_Vectors) ; 


procedure  Update_X  is  new  Update_State_Vector 

(State_Indices  =>  State_Indices, 

Measurement_Indices  =>  Measurement_Indices, 

Kalman  Filter_Eleraents  =>  Kalman  Filter_Elements, 
H_MatrIces  =>  H_MatrIces, 

H_Row_Vectors  =>  H_Row_Vectors, 

Measurement_Vectors  =>  Measurement_Vectors, 

K_Column_Vectors  =>  K_Column_Vectors, 

Slate  Vectors  =>  State  Vectors); 


procedure  Update 
(P 
X 


Z 

Complicated_H 
Measurement  Variance 


pragma  PAGE; 

in  out  P_Matrices; 
in  out  State_Vectors; 
in  Measurement_Vectors; 
in  H_Matrices; 

in  Measurement  Variance  Vectors)  is 


begin 


for  Measurement_Nuinber  in  Measurement_Indices  loop 


K  :=  ComputeJG  (P  =>  P, 

Measurement_Number  =>  Measurement_Number, 

Complicated_H  <=>  Complicated_H, 

Measurement  Variance  =>  Measurement  Variance); 


Update_P  (P  =>  P, 

Measurement  Number  =>  Measurement  Number, 
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K 

Complicated_H 


=>  K, 

=>  Complicated_H) ; 


Update_X  (X  => 

Z  => 

K  => 

Measurement_Number  => 
(  Complicated  H  => 

( 

* 

<  end  loop; 


X, 

Z, 


K, 

Measurement_Number , 
Complicated_H) ; 


end  Update; 

end  Sequentially_Update_Covariance_Matrix_And_State_Vector; 


) 

» 


) 

\ 

i 


> 
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with  Kalman_Filter_Common_Parts; 

separate  (Kalman_Filter_Complicated_H_Parts) 

package  body  Kalman_Update  is 

package  P_Manager  is  new 

Kalman_Filter_Common_Par ts . Error_Covariance_Matrix_Manager 
(  Phi_Matrices  =>  Phi_Matrices, 

P_And_Q_Ma trices  =>  P_And_Q_Matrices  ); 

package  Update_P_And_X  is  new 

Sequentially  Update  Covariance  Matrix  And  State  Vector 
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(State_Indices  =>  State_Indices, 

Measurement_Indices  =>  Measurement_Indices, 

Kalman_Filter_Elements  =>  Kalman_Filter_Elements, 

P_Matrices  =>  P_And_Q_Mat rices, 

H_Matrices  =>  H_Matrices, 

K_H_Pr°duct_Matrices  =>  K_H_Product_Matrices, 

H_Row_Vectors  =>  H_Row_Vectors, 

Measurement_Variance_Vectors  =>  Measuremen t_Variance_Vectors, 

Measurement_Vectors  =>  Measurement_Vectors, 

K_Column_Vectors  =>  K_Column_Vectors, 

State_Vectors  =>  State_Vectors  ); 

Zero_State_Vector  s  State_Vectors  :=  (others  =>  0.0); 
pragma  PAGE; 


:x 

• 

• 

in  out 

State_Vectors; 

p 

• 

• 

in  out 

P_And_Q_Ma  trices; 

z 

• 

• 

in 

Measuremen  t_Vectors ; 

Complicated_H 

• 

• 

in 

H_Ma trices; 

Measuremen  t_Var iance 

• 

• 

in 

Measuremen t_Variance_Vec tors 

Propagated_Phi 

• 

• 

in 

Phi_Matrices; 

Propagated_Q 

• 

• 

in 

P_And_Q_Mat rices)  is 

begin 


—  Propagate  the  Error  Covariance  Matrix 

P_Manager. Initialize  (Initial_P  =>  P); 

P_Manager. Propagate  (Propagated_Phi  =>  Propagated_Phi, 

Propagated_Q  =>  Propagated_Q) ; 

P  :=  P_Manager.P; 

—  If  the  state  vector  is  not  zero,  multiply  it  by  Propagated  Phi 
if  X  /=  Zero  State  Vector  then 


Propagated_Phi  *  X; 


end  if; 


—  Update  Error  Covariance  Matrix  and  State  Vector 

Update_P_And_X. Update  (P 

X 


=>  P, 

=>  X, 


.  VS 
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Z 

Complicated_H 

Measurement_Variance 

end  Update; 
end  Kalman  Update; 


I 
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Z, 

=>  Complicated_H, 

=>  Measurement  Variance) ; 


\ 

i 

j. 
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jCl>. 


separate  (Kalman_Filter_Complicated_H_Parts) 

procedure  Update_Error_Covariance_Matrix_General_Form 
(P  :  in  out  P  Matrices; 


Measurement 

Number 

:  in 

Measurement  Indices; 

K 

:  in 

K  Column  Vectors; 

Complicated_ 

:  in 

H_Matrices; 

Measurement 

Variance 

:  in 

Measurement_Variance  Vectors  )  is 

A  :  K_H_Product_Matrices; 
V  :  H  Row  Vectors; 


begin 
V  : 

A 

P 


:=  Active_H_Vector  (Source  =>  Complicated_H, 

Row  =>  Measurement_Number); 

:=  Subtract_From_Identity  (K  *  V); 

:=  Aba_Transpose(  A,  P  )  + 

Aba  Transpose(  K,  Measurement  Variance(  Measurement  Number  )  ); 


end  Update  Error  Covariance  Matrix  General  Form; 
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